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:
parent
7038aacdfe
commit
116ec93332
7
.gitignore
vendored
7
.gitignore
vendored
@ -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
|
||||
|
||||
|
18
Makefile
18
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/"
|
||||
|
||||
|
@ -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)
|
||||
|
||||
|
@ -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
|
||||
|
@ -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)
|
@ -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:
|
@ -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")
|
@ -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):
|
@ -31,7 +31,7 @@ import sys
|
||||
import os
|
||||
import inspect
|
||||
|
||||
from openpilot.uavtalk.uavobject import *
|
||||
from librepilot.uavtalk.uavobject import *
|
||||
|
||||
|
||||
class TimeoutException(Exception):
|
@ -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) + "]"
|
@ -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)
|
Loading…
x
Reference in New Issue
Block a user