mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-05 16:46:06 +01:00
372 lines
11 KiB
Python
372 lines
11 KiB
Python
##
|
|
##############################################################################
|
|
#
|
|
# @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) + "]"
|