1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00

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.
This commit is contained in:
padeler 2017-02-16 02:51:50 +02:00
parent 7038aacdfe
commit 116ec93332
16 changed files with 305 additions and 215 deletions

7
.gitignore vendored
View File

@ -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

View File

@ -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/"

View File

@ -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)

View File

@ -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

View File

@ -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)

View File

@ -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:

View File

@ -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")

View File

@ -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):

View File

@ -31,7 +31,7 @@ import sys
import os
import inspect
from openpilot.uavtalk.uavobject import *
from librepilot.uavtalk.uavobject import *
class TimeoutException(Exception):

View File

@ -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) + "]"

View File

@ -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)