rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merged in padeler/librepilot/LP-483-python-uavtalk (pull request #389)

Browse Source 
LP-483 python uavtalk scripts and examples

Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Philippe Renon <philippe_renon@yahoo.fr>
Approved-by: Alessio Morale <alessiomorale@gmail.com>
Approved-by: Pashalis Padeleris <padeler@gmail.com>
This commit is contained in:
Pashalis Padeleris 2017-03-23 19:44:56 +00:00 committed by Lalanne Laurent
commit 67a9aa35ef
17 changed files with 1670 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
.gitignore vendored
View File

@ -65,15 +65,22 @@ ground/gcs/.settings
/.metadata
/.settings
/.pydevproject
# python artifacts
/**/*.py*
/workspace
# Ignore Eclipse temp folder, git plugin based?
RemoteSystemsTempFiles
# Ignore patch-related files
*.rej
*.orig
*.diff~
# ignore ccache storage
#pycharm project files
/python/.idea
# Ignore Eclipse temp folder, git plugin based?
RemoteSystemsTempFiles
# Ignore patch-related files
*.rej
*.orig
*.diff~
# ignore ccache storage
.ccache

21
Makefile
View File

@ -209,6 +209,23 @@ uavobjects_%: $(UAVOBJGENERATOR)
$(UAVOBJGENERATOR) -$* $(UAVOBJ_XML_DIR) $(ROOT_DIR) ; \
)
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)/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)/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/"
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_<group> - 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)"

7
flight/modules/FlightPlan/lib/uavobject.pyt.template
View File

@ -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.
@ -30,14 +31,14 @@
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#
from uavobject import *
from librepilot.uavtalk.uavobject import *
$(DATAFIELDS)
# Object $(NAME) definition
class $(NAME)(UAVObject):
# Object constants
OBJID = $(UOBJID)
OBJID = $(OBJIDHEX)
# Constructor
def __init__(self):

0
python/examples/__init__.py Normal file
View File

219
python/examples/example.py Normal file
View File

@ -0,0 +1,219 @@
##
##############################################################################
#
# @file example.py
# @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
#
#############################################################################/
#
# 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 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
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 = 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 = 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) + " "
# 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 = UAVMetaDataObject.Access.READONLY
self.objMan.ActuatorCommand.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

113
python/examples/example_readlog.py Normal file
View File

@ -0,0 +1,113 @@
##
##############################################################################
#
# @file example_readlog.py
# @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
#
#############################################################################/
#
# 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 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 UavtalkLogDemo(UavtalkDemo):
def __init__(self):
self.nbUpdates = 0
self.lastRateCalc = time.time()
self.updateRate = 0
self.objMan = None
self.connMan = None
def setup(self, 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 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 = UavtalkLogDemo()
demo.setup(filename)
while True:
demo.objMan.AttitudeState.waitUpdate()
demo._onAttitudeUpdate(demo.objMan.AttitudeState)
except KeyboardInterrupt:
pass
except Exception,e:
print
print "An error occured: ", e
print
traceback.print_exc()
print

206
python/examples/example_tuning.py Normal file
View File

@ -0,0 +1,206 @@
##
##############################################################################
#
# @file example.py
# @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
#
#############################################################################/
#
# 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 librepilot.uavtalk.uavobject import *
from librepilot.uavtalk.uavtalk import *
from librepilot.uavtalk.objectManager import *
from librepilot.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)
print "Starting ObjectManager"
objMan = ObjManager(uavTalk)
objMan.importDefinitions()
print "Starting UavTalk"
uavTalk.start()
print "Getting Current Settings:"
for _ in range(2): # Try only twice to get the settings
try:
time.sleep(1)
objMan.StabilizationSettingsBank1.getUpdate()
except TimeoutException:
print "Timeout"
pass
except KeyboardInterrupt:
exit(1)
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 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()
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

0
python/librepilot/__init__.py Normal file
View File

0
python/librepilot/uavobjects/__init__.py Normal file
View File

0
python/librepilot/uavtalk/__init__.py Normal file
View File

105
python/librepilot/uavtalk/connectionManager.py Normal file
View File

@ -0,0 +1,105 @@
##
##############################################################################
#
# @file connectionManager.py
# @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
#
#############################################################################/
#
# 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 librepilot.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 = 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
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")
else:
if connected:
logging.debug("CONNECTED")
self.connected = connected

126
python/librepilot/uavtalk/flighttelemetrystats.py Normal file
View File

@ -0,0 +1,126 @@
##
##############################################################################
#
# @file flighttelemetrystats.py
# @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.
#
# @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 librepilot.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()

154
python/librepilot/uavtalk/objectManager.py Normal file
View File

@ -0,0 +1,154 @@
##
##############################################################################
#
# @file objectManager.py
# @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
#
#############################################################################/
#
# 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 librepilot.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)

371
python/librepilot/uavtalk/uavobject.py Normal file
View File

@ -0,0 +1,371 @@
##
##############################################################################
#
# @file uavobject.py
# @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
#
#############################################################################/
#
# 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
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
def _set_bits(var,shift,value,mask):
return (var & ~(mask << shift)) | (value << shift)
class UAVObjectField(object):
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):
# 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]
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(object):
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 MetaFlagsField(UAVObjectField):
def __init__(self):
UAVObjectField.__init__(self, UAVObjectField.FType.UINT16, 1)
class MetaFlightTelemetryUpdatePeriod(UAVObjectField):
def __init__(self):
UAVObjectField.__init__(self, UAVObjectField.FType.UINT16, 1)
class MetaGCSTelemetryUpdatePeriod(UAVObjectField):
def __init__(self):
UAVObjectField.__init__(self, UAVObjectField.FType.UINT16, 1)
class MetaLoggingUpdatePeriod(UAVObjectField):
def __init__(self):
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:
MANUAL = 0
PERIODIC = 1
ONCHANGE = 2
THROTTLED = 3
class Access:
READWRITE = 0
READONLY = 1
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.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.telemetryUpdatePeriod = MetaFlightTelemetryUpdatePeriod()
self.addField(self.telemetryUpdatePeriod)
self.gcsTelemetryUpdatePeriod = MetaGCSTelemetryUpdatePeriod()
self.addField(self.gcsTelemetryUpdatePeriod)
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) + "]"

315
python/librepilot/uavtalk/uavtalk.py Normal file
View File

@ -0,0 +1,315 @@
##
##############################################################################
#
# @file uavtalk.py
# @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
#
#############################################################################/
#
# 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
from librepilot.uavtalk.objectManager import TimeoutException
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, "rb")
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. 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
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. 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
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.debug("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, obj_type, obj_id, data=None):
# name = self.objMan.objs[obj_id].name
# logging.debug("Enter lock " + str(obj_type) + " " + name)
self.txLock.acquire()
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)

10
python/pyuavtalk.iml Normal file
View File

@ -0,0 +1,10 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager" inherit-compiler-output="true">
<exclude-output />
<content url="file://$MODULE_DIR$" />
<orderEntry type="jdk" jdkName="Python 2.7.9 virtualenv at ~/workspace/python/uavtalk" jdkType="Python SDK" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
</module>

9
python/setup.py Normal file
View File

@ -0,0 +1,9 @@
from distutils.core import setup
import glob
setup(name='LibrePilot UAVTalk',
version='1.0',
description='LibrePilot UAVTalk',
url='http://www.librepilot.org',
packages=['librepilot', 'librepilot.uavtalk', 'librepilot.uavobjects'],
)