From 3489b0c2ab4e66a1f95821753340acb78172d933 Mon Sep 17 00:00:00 2001 From: vassilis Date: Wed, 10 Mar 2010 13:25:12 +0000 Subject: [PATCH] Initial release of UAVObjects and UAVTalk GCS plugins (work in progress) git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@277 ebee16cc-31ac-478f-84a7-5cbb03baadba --- ground/src/plugins/plugins.pro | 13 +- .../src/plugins/uavobjects/uavdataobject.cpp | 87 +++ ground/src/plugins/uavobjects/uavdataobject.h | 40 ++ .../src/plugins/uavobjects/uavmetaobject.cpp | 58 ++ ground/src/plugins/uavobjects/uavmetaobject.h | 28 + ground/src/plugins/uavobjects/uavobject.cpp | 68 +++ ground/src/plugins/uavobjects/uavobject.h | 72 +++ .../src/plugins/uavobjects/uavobjectfield.cpp | 203 +++++++ .../src/plugins/uavobjects/uavobjectfield.h | 57 ++ .../plugins/uavobjects/uavobjectmanager.cpp | 275 ++++++++++ .../src/plugins/uavobjects/uavobjectmanager.h | 44 ++ .../plugins/uavobjects/uavobjects.pluginspec | 10 + ground/src/plugins/uavobjects/uavobjects.pro | 19 + .../src/plugins/uavobjects/uavobjectsinit.cpp | 6 + .../src/plugins/uavobjects/uavobjectsinit.h | 8 + .../plugins/uavobjects/uavobjectsplugin.cpp | 35 ++ .../src/plugins/uavobjects/uavobjectsplugin.h | 19 + .../plugins/uavobjects/uavobjecttemplate.cpp | 33 ++ .../plugins/uavobjects/uavobjecttemplate.h | 28 + ground/src/plugins/uavtalk/telemetry.cpp | 0 ground/src/plugins/uavtalk/telemetry.h | 0 ground/src/plugins/uavtalk/uavtalk.cpp | 510 ++++++++++++++++++ ground/src/plugins/uavtalk/uavtalk.h | 73 +++ ground/src/plugins/uavtalk/uavtalk.pluginspec | 10 + ground/src/plugins/uavtalk/uavtalk.pro | 11 + ground/src/plugins/uavtalk/uavtalkplugin.cpp | 5 + ground/src/plugins/uavtalk/uavtalkplugin.h | 10 + 27 files changed, 1721 insertions(+), 1 deletion(-) create mode 100644 ground/src/plugins/uavobjects/uavdataobject.cpp create mode 100644 ground/src/plugins/uavobjects/uavdataobject.h create mode 100644 ground/src/plugins/uavobjects/uavmetaobject.cpp create mode 100644 ground/src/plugins/uavobjects/uavmetaobject.h create mode 100644 ground/src/plugins/uavobjects/uavobject.cpp create mode 100644 ground/src/plugins/uavobjects/uavobject.h create mode 100644 ground/src/plugins/uavobjects/uavobjectfield.cpp create mode 100644 ground/src/plugins/uavobjects/uavobjectfield.h create mode 100644 ground/src/plugins/uavobjects/uavobjectmanager.cpp create mode 100644 ground/src/plugins/uavobjects/uavobjectmanager.h create mode 100644 ground/src/plugins/uavobjects/uavobjects.pluginspec create mode 100644 ground/src/plugins/uavobjects/uavobjects.pro create mode 100644 ground/src/plugins/uavobjects/uavobjectsinit.cpp create mode 100644 ground/src/plugins/uavobjects/uavobjectsinit.h create mode 100644 ground/src/plugins/uavobjects/uavobjectsplugin.cpp create mode 100644 ground/src/plugins/uavobjects/uavobjectsplugin.h create mode 100644 ground/src/plugins/uavobjects/uavobjecttemplate.cpp create mode 100644 ground/src/plugins/uavobjects/uavobjecttemplate.h create mode 100644 ground/src/plugins/uavtalk/telemetry.cpp create mode 100644 ground/src/plugins/uavtalk/telemetry.h create mode 100644 ground/src/plugins/uavtalk/uavtalk.cpp create mode 100644 ground/src/plugins/uavtalk/uavtalk.h create mode 100644 ground/src/plugins/uavtalk/uavtalk.pluginspec create mode 100644 ground/src/plugins/uavtalk/uavtalk.pro create mode 100644 ground/src/plugins/uavtalk/uavtalkplugin.cpp create mode 100644 ground/src/plugins/uavtalk/uavtalkplugin.h diff --git a/ground/src/plugins/plugins.pro b/ground/src/plugins/plugins.pro index 58a7178e8..78d4f978a 100644 --- a/ground/src/plugins/plugins.pro +++ b/ground/src/plugins/plugins.pro @@ -4,7 +4,7 @@ TEMPLATE = subdirs SUBDIRS = plugin_coreplugin \ - plugin_welcome + plugin_welcome \ # Blank Template Plugin, not compiled by default #SUBDIRS += plugin_donothing @@ -18,4 +18,15 @@ plugin_coreplugin.subdir = coreplugin plugin_welcome.subdir = welcome plugin_welcome.depends = plugin_coreplugin +# UAVObjects plug-in +#SUBDIRS += plugin_uavobjects +#plugin_uavobjects.subdir = uavobjects +#plugin_uavobjects.depends = plugin_coreplugin + +# UAVTalk plug-in +#SUBDIRS += plugin_uavtalk +#plugin_uavtalk.subdir = uavtalk +#plugin_uavtalk.depends = plugin_uavobjects +#plugin_uavtalk.depends += plugin_coreplugin + diff --git a/ground/src/plugins/uavobjects/uavdataobject.cpp b/ground/src/plugins/uavobjects/uavdataobject.cpp new file mode 100644 index 000000000..e7f9391c3 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavdataobject.cpp @@ -0,0 +1,87 @@ +#include "uavdataobject.h" + +UAVDataObject::UAVDataObject(quint32 objID, quint32 instID, bool isSingleInst, QString& name, quint32 numBytes): + UAVObject(objID, instID, isSingleInst, name, numBytes) +{ + data = new quint8[numBytes]; // create data buffer, object fields are stored there as a packed structure + mobj = NULL; +} + +void UAVDataObject::initialize(QList fields, Metadata& mdata) +{ + UAVMetaObject* obj = new UAVMetaObject(objID+1, name.append("Meta"), mdata, this); + initialize(fields, obj); +} + +void UAVDataObject::initialize(QList fields, UAVMetaObject* mobj) +{ + // Initialize fields and create data buffer + this->mobj = mobj; + this->fields = fields; + quint32 offset = 0; + for (int n = 0; n < fields.length(); ++n) + { + fields.value(n)->initialize(data, offset, this); + offset += fields.value(n)->getNumBytes(); + connect(fields.value(n), SIGNAL(fieldUpdated(UAVObjectField*)), this, SLOT(fieldUpdated(UAVObjectField*))); + } +} + +void UAVDataObject::fieldUpdated(UAVObjectField* field) +{ + emit objectUpdated(this, false); // trigger object updated event +} + +qint32 UAVDataObject::getNumFields() +{ + return fields.count(); +} + +QList UAVDataObject::getFields() +{ + return fields; +} + +UAVObjectField* UAVDataObject::getField(QString& name) +{ + // Look for field + for (int n = 0; n < fields.length(); ++n) + { + if (name.compare(fields.value(n)->getName()) == 0) + { + return fields.value(n); + } + } + // If this point is reached then the field was not found + return NULL; +} + +qint32 UAVDataObject::pack(quint8* dataOut) +{ + QMutexLocker locker(mutex); + memcpy(dataOut, data, numBytes); + return numBytes; +} + +qint32 UAVDataObject::unpack(const quint8* dataIn) +{ + QMutexLocker locker(mutex); + memcpy(data, dataIn, numBytes); + emit objectUpdated(this, true); // trigger object updated event + return numBytes; +} + +void UAVDataObject::setMetadata(const Metadata& mdata) +{ + mobj->setData(mdata); +} + +UAVObject::Metadata UAVDataObject::getMetadata(void) +{ + return mobj->getData(); +} + +UAVMetaObject* UAVDataObject::getMetaObject() +{ + return mobj; +} diff --git a/ground/src/plugins/uavobjects/uavdataobject.h b/ground/src/plugins/uavobjects/uavdataobject.h new file mode 100644 index 000000000..13a8cd836 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavdataobject.h @@ -0,0 +1,40 @@ +#ifndef UAVDATAOBJECT_H +#define UAVDATAOBJECT_H + +#include "uavobject.h" +#include "uavobjectfield.h" +#include "uavmetaobject.h" +#include + +class UAVDataObject: public UAVObject +{ + Q_OBJECT + +public: + UAVDataObject(quint32 objID, quint32 instID, bool isSingleInst, QString& name, quint32 numBytes); + void initialize(QList fields, Metadata& mdata); + void initialize(QList fields, UAVMetaObject* mobj); + + qint32 getNumFields(); + QList getFields(); + UAVObjectField* getField(QString& name); + qint32 pack(quint8* dataOut); + qint32 unpack(const quint8* dataIn); + void setMetadata(const Metadata& mdata); + Metadata getMetadata(); + UAVMetaObject* getMetaObject(); + +private slots: + void fieldUpdated(UAVObjectField* field); + +private: + quint8* data; + QList fields; + UAVMetaObject* mobj; + + void initialize(QList fields); + + +}; + +#endif // UAVDATAOBJECT_H diff --git a/ground/src/plugins/uavobjects/uavmetaobject.cpp b/ground/src/plugins/uavobjects/uavmetaobject.cpp new file mode 100644 index 000000000..cbf6cf587 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavmetaobject.cpp @@ -0,0 +1,58 @@ +#include "UAVMetaObject.h" + +UAVMetaObject::UAVMetaObject(quint32 objID, QString& name, Metadata& mdata, UAVObject* parent): + UAVObject(objID, 0, true, name, sizeof(Metadata)) +{ + this->parentMetadata = mdata; + this->parent = parent; + ownMetadata.ackRequired = 1; + ownMetadata.flightTelemetryUpdateMode = UPDATEMODE_ONCHANGE; + ownMetadata.flightTelemetryUpdatePeriod = 0; + ownMetadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE; + ownMetadata.gcsTelemetryUpdatePeriod = 0; + ownMetadata.loggingUpdateMode = UPDATEMODE_ONCHANGE; + ownMetadata.loggingUpdatePeriod = 0; +} + +UAVObject* UAVMetaObject::getParentObject() +{ + return parent; +} + +qint32 UAVMetaObject::pack(quint8* dataOut) +{ + QMutexLocker locker(mutex); + memcpy(dataOut, &parentMetadata, sizeof(Metadata)); + return sizeof(Metadata); +} + +qint32 UAVMetaObject::unpack(const quint8* dataIn) +{ + QMutexLocker locker(mutex); + memcpy(&parentMetadata, dataIn, sizeof(Metadata)); + emit objectUpdated(this, true); // trigger object updated event + return sizeof(Metadata); +} + +void UAVMetaObject::setMetadata(const Metadata& mdata) +{ + return; // can not update metaobject's metadata +} + +UAVObject::Metadata UAVMetaObject::getMetadata() +{ + return ownMetadata; +} + +void UAVMetaObject::setData(const Metadata& mdata) +{ + QMutexLocker locker(mutex); + parentMetadata = mdata; + emit objectUpdated(this, false); // trigger object updated event +} + +UAVObject::Metadata UAVMetaObject::getData() +{ + QMutexLocker locker(mutex); + return parentMetadata; +} diff --git a/ground/src/plugins/uavobjects/uavmetaobject.h b/ground/src/plugins/uavobjects/uavmetaobject.h new file mode 100644 index 000000000..6c01aeb44 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavmetaobject.h @@ -0,0 +1,28 @@ +#ifndef UAVMETAOBJECT_H +#define UAVMETAOBJECT_H + +#include "UAVObject.h" + +class UAVMetaObject: public UAVObject +{ + Q_OBJECT + +public: + UAVMetaObject(quint32 objID, QString& name, Metadata& mdata, UAVObject* parent); + UAVObject* getParentObject(); + + qint32 pack(quint8* dataOut); + qint32 unpack(const quint8* dataIn); + void setMetadata(const Metadata& mdata); + Metadata getMetadata(); + void setData(const Metadata& mdata); + Metadata getData(); + +private: + UAVObject* parent; + Metadata ownMetadata; + Metadata parentMetadata; + +}; + +#endif // UAVMETAOBJECT_H diff --git a/ground/src/plugins/uavobjects/uavobject.cpp b/ground/src/plugins/uavobjects/uavobject.cpp new file mode 100644 index 000000000..5251b387b --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobject.cpp @@ -0,0 +1,68 @@ +#include "uavobject.h" + + +UAVObject::UAVObject(quint32 objID, quint32 instID, bool isSingleInst, QString& name, quint32 numBytes): QObject() +{ + this->objID = objID; + this->instID = instID; + this->isSingleInst = isSingleInst; + this->name = name; + this->numBytes = numBytes; + this->mutex = new QMutex(QMutex::Recursive); +} + +quint32 UAVObject::getObjID() +{ + return objID; +} + +quint32 UAVObject::getInstID() +{ + return instID; +} + +bool UAVObject::isSingleInstance() +{ + return isSingleInst; +} + +QString UAVObject::getName() +{ + return name; +} + +quint32 UAVObject::getNumBytes() +{ + return numBytes; +} + +void UAVObject::requestUpdate() +{ + emit updateRequested(this); +} + +void UAVObject::updated() +{ + emit objectUpdated(this, false); +} + +void UAVObject::lock() +{ + mutex->lock(); +} + +void UAVObject::lock(int timeoutMs) +{ + mutex->tryLock(timeoutMs); +} + +void UAVObject::unlock() +{ + mutex->unlock(); +} + +QMutex* UAVObject::getMutex() +{ + return mutex; +} + diff --git a/ground/src/plugins/uavobjects/uavobject.h b/ground/src/plugins/uavobjects/uavobject.h new file mode 100644 index 000000000..274812bdb --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobject.h @@ -0,0 +1,72 @@ +#ifndef UAVOBJECT_H +#define UAVOBJECT_H + +#include +#include +#include +#include +#include + +class UAVObject: public QObject +{ + Q_OBJECT + +public: + + /** + * Object update mode + */ + typedef enum { + UPDATEMODE_PERIODIC = 0, /** Automatically update object at periodic intervals */ + UPDATEMODE_ONCHANGE, /** Only update object when its data changes */ + UPDATEMODE_MANUAL, /** Manually update object, by calling the updated() function */ + UPDATEMODE_NEVER /** Object is never updated */ + } UpdateMode; + + /** + * Object metadata, each object has a meta object that holds its metadata. The metadata define + * properties for each object and can be used by multiple modules (e.g. telemetry and logger) + */ + typedef struct { + quint8 ackRequired; /** Defines if an ack is required for the transactions of this object (1:acked, 0:not acked) */ + UpdateMode flightTelemetryUpdateMode; /** Update mode used by the autopilot */ + qint32 flightTelemetryUpdatePeriod; /** Update period used by the autopilot (only if telemetry mode is PERIODIC) */ + UpdateMode gcsTelemetryUpdateMode; /** Update mode used by the GCS */ + qint32 gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */ + UpdateMode loggingUpdateMode; /** Update mode used by the logging module */ + qint32 loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ + } Metadata; + + + UAVObject(quint32 objID, quint32 instID, bool isSingleInst, QString& name, quint32 numBytes); + quint32 getObjID(); + quint32 getInstID(); + bool isSingleInstance(); + QString getName(); + quint32 getNumBytes(); + virtual qint32 pack(quint8* dataOut) = 0; + virtual qint32 unpack(const quint8* dataIn) = 0; + virtual void setMetadata(const Metadata& mdata) = 0; + virtual Metadata getMetadata() = 0; + void requestUpdate(); + void updated(); + void lock(); + void lock(int timeoutMs); + void unlock(); + QMutex* getMutex(); + +signals: + void objectUpdated(UAVObject* obj, bool unpacked); + void updateRequested(UAVObject* obj); + +protected: + quint32 objID; + quint32 instID; + bool isSingleInst; + QString name; + quint32 numBytes; + QMutex* mutex; + +}; + +#endif // UAVOBJECT_H diff --git a/ground/src/plugins/uavobjects/uavobjectfield.cpp b/ground/src/plugins/uavobjects/uavobjectfield.cpp new file mode 100644 index 000000000..c768c5eca --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjectfield.cpp @@ -0,0 +1,203 @@ +#include "uavobjectfield.h" +#include + +UAVObjectField::UAVObjectField(QString& name, QString& units, FieldType type, quint32 numElements) +{ + // Copy params + this->name = name; + this->units = units; + this->type = type; + this->numElements = numElements; + this->offset = 0; + this->data = NULL; + this->obj = NULL; + // Calculate the number of bytes per element based on the type + switch (type) { + case FIELDTYPE_CHAR: + case FIELDTYPE_INT8: + this->numBytesPerElement = 1; + break; + case FIELDTYPE_INT16: + this->numBytesPerElement = 2; + break; + case FIELDTYPE_INT32: + case FIELDTYPE_FLOAT32: + this->numBytesPerElement = 4; + break; + default: + this->numBytesPerElement = 0; + } +} + +void UAVObjectField::initialize(quint8* data, quint32 dataOffset, UAVObject* obj) +{ + this->data = data; + this->offset = dataOffset; + this->obj = obj; + clear(); +} + +UAVObject* UAVObjectField::getObject() +{ + return obj; +} + +void UAVObjectField::clear() +{ + if (data != NULL) + { + QMutexLocker locker(obj->getMutex()); + for (unsigned int n = 0; n < numBytesPerElement*numElements; ++n) + { + data[offset + n] = 0; + } + } +} + +QString UAVObjectField::getName() +{ + return name; +} + +QString UAVObjectField::getUnits() +{ + return units; +} + +UAVObjectField::FieldType UAVObjectField::getType() +{ + return type; +} + +quint32 UAVObjectField::getNumElements() +{ + return numElements; +} + + +double UAVObjectField::getValue(quint32 index) +{ + double ret = 0.0; + + // Check if index is out of bounds or no data available + if (index < numElements && data != NULL) + { + // Get value from data + QMutexLocker locker(obj->getMutex()); + switch (type) { + case FIELDTYPE_CHAR: + case FIELDTYPE_INT8: + qint8 value8; + value8 = data[offset + numBytesPerElement*index]; + ret = (double)value8; + break; + case FIELDTYPE_INT16: + qint16 value16; + value16 = (qint16)qFromBigEndian(&data[offset + numBytesPerElement*index]); + ret = (double)value16; + break; + case FIELDTYPE_INT32: + qint32 value32; + value32 = (qint32)qFromBigEndian(&data[offset + numBytesPerElement*index]); + ret = (double)value32; + break; + case FIELDTYPE_FLOAT32: + qint32 tmp; + float valuef; + tmp = (qint32)qFromBigEndian(&data[offset + numBytesPerElement*index]); + memcpy(&valuef, &tmp, 4); + ret = (double)valuef; + break; + default: + ret = 0.0; + } + } + return ret; +} + +void UAVObjectField::setValue(double value, quint32 index) +{ + // Check if index is out of bounds or no data available + if (index < numElements && data != NULL) + { + // Set value + QMutexLocker locker(obj->getMutex()); + switch (type) { + case FIELDTYPE_CHAR: + case FIELDTYPE_INT8: + data[offset + numBytesPerElement*index] = (qint8)value; + break; + case FIELDTYPE_INT16: + qToBigEndian((qint16)value, &data[offset + numBytesPerElement*index]); + break; + case FIELDTYPE_INT32: + qToBigEndian((qint32)value, &data[offset + numBytesPerElement*index]); + break; + case FIELDTYPE_FLOAT32: + qint32 tmp; + memcpy(&tmp, &value, 4); + qToBigEndian((qint32)tmp, &data[offset + numBytesPerElement*index]); + break; + } + } + + // Emit updated event + emit fieldUpdated(this); +} + +double UAVObjectField::getValue() +{ + return getValue(0); +} + +void UAVObjectField::setValue(double value) +{ + setValue(value, 0); +} + + +QString UAVObjectField::getString() +{ + QString str; + if (data != NULL) + { + QMutexLocker locker(obj->getMutex()); + data[offset + numElements - 1] = '\0'; // null terminate + if (type == FIELDTYPE_CHAR) + { + str.append((char*)&data[offset]); + } + } + return str; +} + +void UAVObjectField::setString(QString& str) +{ + QByteArray barray = str.toAscii(); + if (data != NULL) + { + QMutexLocker locker(obj->getMutex()); + for (int n = 0; n < barray.length() && n < (int)numElements; ++n) + { + data[offset+n] = barray[n]; + } + data[offset + numElements - 1] = '\0'; // null terminate + } +} + +quint32 UAVObjectField::getDataOffset() +{ + return offset; +} + +quint32 UAVObjectField::getNumBytes() +{ + return numBytesPerElement * numElements; +} + +quint32 UAVObjectField::getNumBytesElement() +{ + return numBytesPerElement; +} + + diff --git a/ground/src/plugins/uavobjects/uavobjectfield.h b/ground/src/plugins/uavobjects/uavobjectfield.h new file mode 100644 index 000000000..a36e6c4d0 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjectfield.h @@ -0,0 +1,57 @@ +#ifndef UAVOBJECTFIELD_H +#define UAVOBJECTFIELD_H + +#include "uavobject.h" + +class UAVObjectField: public QObject +{ + Q_OBJECT + +public: + /** + * Recognized field types + */ + typedef enum { + FIELDTYPE_INT8 = 0, + FIELDTYPE_INT16, + FIELDTYPE_INT32, + FIELDTYPE_FLOAT32, + FIELDTYPE_CHAR + } FieldType; + + UAVObjectField(QString& name, QString& units, FieldType type, quint32 numElements); + void initialize(quint8* data, quint32 dataOffset, UAVObject* obj); + UAVObject* getObject(); + QString getName(); + QString getUnits(); + FieldType getType(); + quint32 getNumElements(); + double getValue(); + void setValue(double value); + double getValue(quint32 index); + void setValue(double value, quint32 index); + QString getString(); + void setString(QString& str); + quint32 getDataOffset(); + quint32 getNumBytes(); + quint32 getNumBytesElement(); + +signals: + void fieldUpdated(UAVObjectField* field); + +private: + QString name; + QString units; + FieldType type; + quint32 numElements; + quint32 numBytesPerElement; + quint32 offset; + quint8* data; + UAVObject* obj; + + void clear(); + + +}; + +#endif // UAVOBJECTFIELD_H diff --git a/ground/src/plugins/uavobjects/uavobjectmanager.cpp b/ground/src/plugins/uavobjects/uavobjectmanager.cpp new file mode 100644 index 000000000..67d1452e3 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjectmanager.cpp @@ -0,0 +1,275 @@ +#include "uavobjectmanager.h" + +UAVObjectManager::UAVObjectManager() +{ + mutex = new QMutex(QMutex::Recursive); +} + +bool UAVObjectManager::registerObject(UAVObject* obj) +{ + QMutexLocker locker(mutex); + // Check if this object type is already in the list + for (int objidx = 0; objidx < objects.length(); ++objidx) + { + // Check if the object ID is in the list + if (objects[objidx].length() > 0 && objects[objidx][0]->getObjID() == obj->getObjID()) + { + // Check if the instance is already in the list + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) + { + if ( objects[objidx][instidx]->getInstID() == obj->getInstID() ) + { + // Object already registered, do not add + return false; + } + } + // If this point is reached, the object is not in the list, so now we can add it in the existing list + objects[objidx].append(obj); + emit newObject(obj); + return true; + } + } + // If this point is reached then this is the first time this object ID is added in the list + // create a new list of the instances and add in the object collection. + QList list; + list.append(obj); + objects.append(list); + emit newObject(obj); + return true; +} + +UAVDataObject* UAVObjectManager::newObjectInstance(QString& name, quint32 instId) +{ + return newObjectInstance(&name, 0, instId); +} + +UAVDataObject* UAVObjectManager::newObjectInstance(quint32 objId, quint32 instId) +{ + return newObjectInstance(NULL, objId, instId); +} + +UAVDataObject* UAVObjectManager::newObjectInstance(QString* name, quint32 objId, quint32 instId) +{ + QMutexLocker locker(mutex); + // Get object of the same name from collection + UAVObject* tmpObj; + if (name != NULL) + { + tmpObj = getObject(*name); + } + else + { + tmpObj = getObject(objId); + } + if (tmpObj == NULL) + { + return NULL; + } + // Make sure this is a data object + UAVDataObject* refObj = dynamic_cast(tmpObj); + if (refObj == NULL) + { + return NULL; + } + // Check if this is single instance object + if (refObj->isSingleInstance()) + { + return NULL; + } + // Make a deep copy of the fields in the reference object + QList fields; + QList refFields = refObj->getFields(); + for (int n = 0; n < refFields.length(); ++n) + { + QString fname = refFields[n]->getName(); + QString funits = refFields[n]->getUnits(); + UAVObjectField* field = new UAVObjectField(fname, funits, refFields[n]->getType(), + refFields[n]->getNumElements()); + + fields.append(field); + } + // Calculate instance ID, if the one specified is 0 + // (the first object registered always gets the instance ID of zero, so any new instances will be >1) + if (instId == 0) + { + instId = getNumInstances(tmpObj->getObjID()) + 1; + } + // Create new instance, by using properties from reference object + QString oname = refObj->getName(); + UAVDataObject* obj = new UAVDataObject(refObj->getObjID(), instId, refObj->isSingleInstance(), + oname, refObj->getNumBytes()); + obj->initialize(fields, refObj->getMetaObject()); + // Register + registerObject(obj); + // Trigger update + obj->updated(); + return obj; +} + +QList< QList > UAVObjectManager::getObjects() +{ + QMutexLocker locker(mutex); + return objects; +} + +QList< QList > UAVObjectManager::getDataObjects() +{ + QMutexLocker locker(mutex); + QList< QList > dObjects; + + // Go through objects and copy to new list when types match + for (int objidx = 0; objidx < objects.length(); ++objidx) + { + if (objects[objidx].length() > 0) + { + // Check type + UAVDataObject* obj = dynamic_cast(objects[objidx][0]); + if (obj != NULL) + { + // Create instance list + QList list; + // Go through instances and cast them to UAVDataObject, then add to list + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) + { + obj = dynamic_cast(objects[objidx][instidx]); + if (obj != NULL) + { + list.append(obj); + } + } + // Append to object list + dObjects.append(list); + } + } + } + // Done + return dObjects; +} + +QList > UAVObjectManager::getMetaObjects() +{ + QMutexLocker locker(mutex); + QList< QList > mObjects; + + // Go through objects and copy to new list when types match + for (int objidx = 0; objidx < objects.length(); ++objidx) + { + if (objects[objidx].length() > 0) + { + // Check type + UAVMetaObject* obj = dynamic_cast(objects[objidx][0]); + if (obj != NULL) + { + // Create instance list + QList list; + // Go through instances and cast them to UAVMetaObject, then add to list + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) + { + obj = dynamic_cast(objects[objidx][instidx]); + if (obj != NULL) + { + list.append(obj); + } + } + // Append to object list + mObjects.append(list); + } + } + } + // Done + return mObjects; +} + +UAVObject* UAVObjectManager::getObject(QString& name, quint32 instId) +{ + return getObject(&name, 0, instId); +} + +UAVObject* UAVObjectManager::getObject(quint32 objId, quint32 instId) +{ + return getObject(NULL, objId, instId); +} + +UAVObject* UAVObjectManager::getObject(QString* name, quint32 objId, quint32 instId) +{ + QMutexLocker locker(mutex); + // Check if this object type is already in the list + for (int objidx = 0; objidx < objects.length(); ++objidx) + { + // Check if the object ID is in the list + if (objects[objidx].length() > 0) + { + if ( (name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId) ) + { + // Look for the requested instance ID + for (int instidx = 0; instidx < objects[objidx].length(); ++instidx) + { + if (objects[objidx][instidx]->getInstID() == instId) + { + return objects[objidx][instidx]; + } + } + } + } + } + // If this point is reached then the requested object could not be found + return NULL; +} + +QList UAVObjectManager::getObjectInstances(QString& name) +{ + return getObjectInstances(&name, 0); +} + +QList UAVObjectManager::getObjectInstances(quint32 objId) +{ + return getObjectInstances(NULL, objId); +} + +QList UAVObjectManager::getObjectInstances(QString* name, quint32 objId) +{ + QMutexLocker locker(mutex); + // Check if this object type is already in the list + for (int objidx = 0; objidx < objects.length(); ++objidx) + { + // Check if the object ID is in the list + if (objects[objidx].length() > 0) + { + if ( (name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId) ) + { + return objects[objidx]; + } + } + } + // If this point is reached then the requested object could not be found + return QList(); +} + +qint32 UAVObjectManager::getNumInstances(QString& name) +{ + return getNumInstances(&name, 0); +} + +qint32 UAVObjectManager::getNumInstances(quint32 objId) +{ + return getNumInstances(NULL, objId); +} + +qint32 UAVObjectManager::getNumInstances(QString* name, quint32 objId) +{ + QMutexLocker locker(mutex); + // Check if this object type is already in the list + for (int objidx = 0; objidx < objects.length(); ++objidx) + { + // Check if the object ID is in the list + if (objects[objidx].length() > 0) + { + if ( (name != NULL && objects[objidx][0]->getName().compare(name) == 0) || (name == NULL && objects[objidx][0]->getObjID() == objId) ) + { + return objects[objidx].length(); + } + } + } + // If this point is reached then the requested object could not be found + return -1; +} diff --git a/ground/src/plugins/uavobjects/uavobjectmanager.h b/ground/src/plugins/uavobjects/uavobjectmanager.h new file mode 100644 index 000000000..f09d46bc1 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjectmanager.h @@ -0,0 +1,44 @@ +#ifndef UAVOBJECTMANAGER_H +#define UAVOBJECTMANAGER_H + +#include "uavobject.h" +#include "uavdataobject.h" +#include "uavmetaobject.h" +#include +#include + +class UAVObjectManager: public QObject +{ + Q_OBJECT + +public: + UAVObjectManager(); + + bool registerObject(UAVObject* obj); + UAVDataObject* newObjectInstance(QString& name, quint32 instId = 0); + UAVDataObject* newObjectInstance(quint32 objId, quint32 instId = 0); + QList< QList > getObjects(); + QList< QList > getDataObjects(); + QList< QList > getMetaObjects(); + UAVObject* getObject(QString& name, quint32 instId = 0); + UAVObject* getObject(quint32 objId, quint32 instId = 0); + QList getObjectInstances(QString& name); + QList getObjectInstances(quint32 objId); + qint32 getNumInstances(QString& name); + qint32 getNumInstances(quint32 objId); + +signals: + void newObject(UAVObject* obj); + +private: + QList< QList > objects; + QMutex* mutex; + + UAVDataObject* newObjectInstance(QString* name, quint32 objId, quint32 instId); + UAVObject* getObject(QString* name, quint32 objId, quint32 instId); + QList getObjectInstances(QString* name, quint32 objId); + qint32 getNumInstances(QString* name, quint32 objId); +}; + + +#endif // UAVOBJECTMANAGER_H diff --git a/ground/src/plugins/uavobjects/uavobjects.pluginspec b/ground/src/plugins/uavobjects/uavobjects.pluginspec new file mode 100644 index 000000000..e94047c00 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjects.pluginspec @@ -0,0 +1,10 @@ + + The OpenPilot Project + (C) 2010 OpenPilot Project + Your License goes here + UAV telemetry objects + http://www.openpilot.org + + + + \ No newline at end of file diff --git a/ground/src/plugins/uavobjects/uavobjects.pro b/ground/src/plugins/uavobjects/uavobjects.pro new file mode 100644 index 000000000..051db0ff1 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjects.pro @@ -0,0 +1,19 @@ +TEMPLATE = lib +TARGET = UAVObjects +include(../../openpilotgcsplugin.pri) +include(../../plugins/coreplugin/coreplugin.pri) +HEADERS += uavobject.h \ + uavmetaobject.h \ + uavobjectmanager.h \ + uavdataobject.h \ + uavobjectfield.h \ + uavobjectsinit.h \ + uavobjectsplugin.h +SOURCES += uavobject.cpp \ + uavmetaobject.cpp \ + uavobjectmanager.cpp \ + uavdataobject.cpp \ + uavobjectfield.cpp \ + uavobjectsinit.cpp \ + uavobjectsplugin.cpp +OTHER_FILES += uavobjects.pluginspec diff --git a/ground/src/plugins/uavobjects/uavobjectsinit.cpp b/ground/src/plugins/uavobjects/uavobjectsinit.cpp new file mode 100644 index 000000000..0db780db8 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjectsinit.cpp @@ -0,0 +1,6 @@ +#include "uavobjectsinit.h" + +void UAVObjectsInitialize(UAVObjectManager* objMngr) +{ + //objMngr->registerObject( new TestObject() ); +} diff --git a/ground/src/plugins/uavobjects/uavobjectsinit.h b/ground/src/plugins/uavobjects/uavobjectsinit.h new file mode 100644 index 000000000..a12be2c55 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjectsinit.h @@ -0,0 +1,8 @@ +#ifndef UAVOBJECTSINIT_H +#define UAVOBJECTSINIT_H + +#include "uavobjectmanager.h" + +void UAVObjectsInitialize(UAVObjectManager* objMngr); + +#endif // UAVOBJECTSINIT_H diff --git a/ground/src/plugins/uavobjects/uavobjectsplugin.cpp b/ground/src/plugins/uavobjects/uavobjectsplugin.cpp new file mode 100644 index 000000000..2b71f416c --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjectsplugin.cpp @@ -0,0 +1,35 @@ +#include "uavobjectsplugin.h" +#include "uavobjectsinit.h" + +UAVObjectsPlugin::UAVObjectsPlugin() +{ + +} + +UAVObjectsPlugin::~UAVObjectsPlugin() +{ + +} + +void UAVObjectsPlugin::extensionsInitialized() +{ + +} + +bool UAVObjectsPlugin::initialize(const QStringList & arguments, QString * errorString) +{ + // Create object manager and expose object + UAVObjectManager* objMngr = new UAVObjectManager(); + addObject(objMngr); + // Initialize UAVObjects + UAVObjectsInitialize(objMngr); + // Done + Q_UNUSED(arguments); + Q_UNUSED(errorString); + return true; +} + +void UAVObjectsPlugin::shutdown() +{ + +} diff --git a/ground/src/plugins/uavobjects/uavobjectsplugin.h b/ground/src/plugins/uavobjects/uavobjectsplugin.h new file mode 100644 index 000000000..6f8ecbf1e --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjectsplugin.h @@ -0,0 +1,19 @@ +#ifndef UAVOBJECTSPLUGIN_H +#define UAVOBJECTSPLUGIN_H + +#include +#include +#include "uavobjectmanager.h" + +class UAVObjectsPlugin: public ExtensionSystem::IPlugin +{ +public: + UAVObjectsPlugin(); + ~UAVObjectsPlugin(); + + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString * errorString); + void shutdown(); +}; + +#endif // UAVOBJECTSPLUGIN_H diff --git a/ground/src/plugins/uavobjects/uavobjecttemplate.cpp b/ground/src/plugins/uavobjects/uavobjecttemplate.cpp new file mode 100644 index 000000000..193a31cd5 --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjecttemplate.cpp @@ -0,0 +1,33 @@ +#include "uavobjecttemplate.h" + +$(NAME)::$(NAME)(): UAVDataObject(OBJID, 0, SINGLEINST, NAME, NUMBYTES) +{ + // Create fields + QList fields; + + $(FIELDS) + // fields.append(new UAVObjectField($(FIELD_NAME), $(FIELD_UNITS), $(FIELD_TYPE), $(FIELD_NUMELEM)); + + // Create metadata + UAVObject::Metadata metadata; + metadata.ackRequired = $(ACK); + metadata.gcsTelemetryUpdateMode = $(GCSTELEM_UPDATEMODE); + metadata.gcsTelemetryUpdatePeriod = $(GCSTELEM_UPDATEPERIOD); + metadata.flightTelemetryUpdateMode = $(FLIGHTTELEM_UPDATEMODE); + metadata.flightTelemetryUpdatePeriod = $(FLIGHTTELEM_UPDATEPERIOD); + metadata.loggingUpdateMode = $(LOGGING_UPDATEMODE); + metadata.loggingUpdatePeriod = $(LOGGING_UPDATEPERIOD); + + // Initialize object + initialize(fields, metadata); +} + +$(NAME)Data $(NAME)::getData() +{ + return data; +} + +void $(NAME)::setData($(NAME)Data& data) +{ + this->data = data; +} diff --git a/ground/src/plugins/uavobjects/uavobjecttemplate.h b/ground/src/plugins/uavobjects/uavobjecttemplate.h new file mode 100644 index 000000000..cd3ed23ec --- /dev/null +++ b/ground/src/plugins/uavobjects/uavobjecttemplate.h @@ -0,0 +1,28 @@ +#ifndef $(NAMEUC)_H +#define $(NAMEUC)_H + +#include "uavdataobject.h" + +class $(NAME): UAVDataObject +{ +public: + static const quint32 OBJID = $(OBJID); + static const QString NAME = QString($(NAME)); + static const bool SINGLEINST = $(SINGLEINST); + static const quint32 NUMBYTES = sizeof($(NAME)Data); + + typedef struct { + $(DATAFIELDS) + } __attribute__((packed)) $(NAME)Data; + + $(NAME)(); + + $(NAME)Data getData(); + void setData($(NAME)Data& data); + +private: + $(NAME)Data data; + +}; + +#endif // $(NAMEUC)_H diff --git a/ground/src/plugins/uavtalk/telemetry.cpp b/ground/src/plugins/uavtalk/telemetry.cpp new file mode 100644 index 000000000..e69de29bb diff --git a/ground/src/plugins/uavtalk/telemetry.h b/ground/src/plugins/uavtalk/telemetry.h new file mode 100644 index 000000000..e69de29bb diff --git a/ground/src/plugins/uavtalk/uavtalk.cpp b/ground/src/plugins/uavtalk/uavtalk.cpp new file mode 100644 index 000000000..4497de904 --- /dev/null +++ b/ground/src/plugins/uavtalk/uavtalk.cpp @@ -0,0 +1,510 @@ +#include "uavtalk.h" +#include + +UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr) +{ + io = iodev; + this->objMngr = objMngr; + rxState = STATE_SYNC; + mutex = new QMutex(QMutex::Recursive); + respSema = new QSemaphore(0); + respObj = NULL; + connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); +} + +void UAVTalk::processInputStream() +{ + quint8 tmp; + + while (io->bytesAvailable() > 0) + { + io->read((char*)&tmp, 1); + processInputByte(tmp); + } +} + +/** + * Request an update for the specified object, on success the object data would have been + * updated by the GCS. + * \param[in] obj Object to update + * \param[in] timeout Time to wait for the response, when zero it will return immediately + * \param[in] allInstances If set true then all instances will be updated + * \return Success (0), Failure (-1) + */ +qint32 UAVTalk::sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances) +{ + return objectTransaction(obj, TYPE_OBJ_REQ, timeout, allInstances); +} + +/** + * Send the specified object through the telemetry link. + * \param[in] obj Object to send + * \param[in] acked Selects if an ack is required + * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately + * \param[in] allInstances If set true then all instances will be updated + * \return 0 Success + * \return -1 Failure + */ +qint32 UAVTalk::sendObject(UAVObject* obj, bool acked, qint32 timeoutMs) +{ + if (acked) + { + return objectTransaction(obj, TYPE_OBJ_ACK, timeoutMs, false); + } + else + { + return objectTransaction(obj, TYPE_OBJ, timeoutMs, false); + } +} + +/** + * Execute the requested transaction on an object. + * \param[in] obj Object + * \param[in] type Transaction type + * TYPE_OBJ: send object, + * TYPE_OBJ_REQ: request object update + * TYPE_OBJ_ACK: send object with an ack + * \param[in] allInstances If set true then all instances will be updated + * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately + * \return Success (true), Failure (false) + */ +bool UAVTalk::objectTransaction(UAVObject* obj, quint8 type, qint32 timeoutMs, bool allInstances) +{ + bool respReceived; + + // Lock + mutex->lock(); + // Send object depending on if a response is needed + if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) + { + if ( transmitObject(obj, type, allInstances) ) + { + respObj = obj; + respAllInstances = allInstances; + mutex->unlock(); // need to release lock since the next call will block until a response is received + respSema->tryAcquire(); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema) + respReceived = respSema->tryAcquire(1, timeoutMs); // lock on object until a response is received (or timeout) + return respReceived; + } + else + { + mutex->unlock(); + return false; + } + } + else if (type == TYPE_OBJ) + { + bool success = transmitObject(obj, TYPE_OBJ, allInstances); + mutex->unlock(); + return success; + } + else + { + mutex->unlock(); + return false; + } +} + +/** + * Process an byte from the telemetry stream. + * \param[in] rxbyte Received byte + * \return Success (true), Failure (false) + */ +bool UAVTalk::processInputByte(quint8 rxbyte) +{ + // Receive state machine + switch (rxState) { + case STATE_SYNC: + if ((rxbyte & TYPE_MASK) == TYPE_VER ) + { + rxCS = rxbyte; + rxType = rxbyte; + rxState = STATE_OBJID; + rxCount = 0; + } + break; + case STATE_OBJID: + rxTmpBuffer[rxCount++] = rxbyte; + if (rxCount == 4) + { + // Search for object, if not found reset state machine + rxObjId = (qint32)qFromBigEndian(rxTmpBuffer); + UAVObject* rxObj = objMngr->getObject(rxObjId); + if (rxObj == NULL) + { + rxState = STATE_SYNC; + } + else + { + // Update checksum + rxCS = updateChecksum(rxCS, rxTmpBuffer, 4); + // Determine data length + if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK) + { + rxLength = 0; + } + else + { + rxLength = rxObj->getNumBytes(); + } + // Check length and determine next state + if (rxLength >= MAX_PAYLOAD_LENGTH) + { + rxState = STATE_SYNC; + } + else + { + // Check if this is a single instance object (i.e. if the instance ID field is coming next) + if ( rxObj->isSingleInstance() ) + { + // If there is a payload get it, otherwise receive checksum + if (rxLength > 0) + { + rxState = STATE_DATA; + } + else + { + rxState = STATE_CS; + } + rxInstId = 0; + rxCount = 0; + } + else + { + rxState = STATE_INSTID; + rxCount = 0; + } + } + } + } + break; + case STATE_INSTID: + rxTmpBuffer[rxCount++] = rxbyte; + if (rxCount == 2) + { + rxInstId = (qint16)qFromBigEndian(rxTmpBuffer); + rxCS = updateChecksum(rxCS, rxTmpBuffer, 2); + rxCount = 0; + // If there is a payload get it, otherwise receive checksum + if (rxLength > 0) + { + rxState = STATE_DATA; + } + else + { + rxState = STATE_CS; + } + } + break; + case STATE_DATA: + rxBuffer[rxCount++] = rxbyte; + if (rxCount == rxLength) + { + rxCS = updateChecksum(rxCS, rxBuffer, rxLength); + rxState = STATE_CS; + rxCount = 0; + } + break; + case STATE_CS: + rxTmpBuffer[rxCount++] = rxbyte; + if (rxCount == 2) + { + rxCSPacket = (qint16)qFromBigEndian(rxTmpBuffer); + if (rxCS == rxCSPacket) + { + mutex->lock(); + receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength); + mutex->unlock(); + } + rxState = STATE_SYNC; + } + break; + default: + rxState = STATE_SYNC; + } + + // Done + return true; +} + +/** + * Receive an object. This function process objects received through the telemetry stream. + * \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK) + * \param[in] obj Handle of the received object + * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. + * \param[in] data Data buffer + * \param[in] length Buffer length + * \return Success (true), Failure (false) + */ +bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* data, qint32 length) +{ + UAVObject* obj = NULL; + bool error = false; + bool allInstances = (instId == ALL_INSTANCES? true : false); + + // Process message type + switch (type) { + case TYPE_OBJ: + // All instances, not allowed for OBJ messages + if (!allInstances) + { + // Get object and update its data + obj = updateObject(objId, instId, data); + // Check if an ack is pending + updateAck(obj); + } + else + { + error = true; + } + break; + case TYPE_OBJ_ACK: + // All instances, not allowed for OBJ_ACK messages + if (!allInstances) + { + // Get object and update its data + obj = updateObject(objId, instId, data); + // Transmit ACK + transmitObject(obj, TYPE_ACK, false); + } + else + { + error = true; + } + break; + case TYPE_OBJ_REQ: + // Get object, if all instances are requested get instance 0 of the object + if (allInstances) + { + obj = objMngr->getObject(objId); + } + else + { + obj = objMngr->getObject(objId, instId); + } + // If object was found transmit it + if (obj != NULL) + { + transmitObject(obj, TYPE_OBJ, allInstances); + } + else + { + error = true; + } + break; + case TYPE_ACK: + // All instances, not allowed for ACK messages + if (!allInstances) + { + // Get object + obj = objMngr->getObject(objId, instId); + // Check if an ack is pending + if (obj != NULL) + { + updateAck(obj); + } + else + { + error = true; + } + } + break; + default: + error = true; + } + // Done + return !error; +} + + +UAVObject* UAVTalk::updateObject(quint32 objId, quint16 instId, quint8* data) +{ + // Get object + UAVObject* obj = objMngr->getObject(objId, instId); + // If the instance does not exist create it + if (obj == NULL) + { + obj = objMngr->newObjectInstance(objId, instId); + } + // Unpack data into object instance + obj->unpack(data); + return obj; +} + +void UAVTalk::updateAck(UAVObject* obj) +{ + if (respObj != NULL && respObj->getObjID() == obj->getObjID() && (respObj->getInstID() == obj->getInstID() || respAllInstances)) + { + respSema->release(); + respObj = NULL; + emit transactionCompleted(obj); + } +} + +/** + * Send an object through the telemetry link. + * \param[in] obj Object to send + * \param[in] type Transaction type + * \param[in] allInstances True is all instances of the object are to be sent + * \return Success (true), Failure (false) + */ +bool UAVTalk::transmitObject(UAVObject* obj, quint8 type, bool allInstances) +{ + // If all instances are requested on a single instance object it is an error + if (allInstances && obj->isSingleInstance()) + { + return false; + } + + // Process message type + if ( type == TYPE_OBJ || type == TYPE_OBJ_ACK ) + { + if (allInstances) + { + // Get number of instances + quint32 numInst = objMngr->getNumInstances(obj->getObjID()); + // Send all instances + for (quint32 instId = 0; instId < numInst; ++instId) + { + UAVObject* inst = objMngr->getObject(obj->getObjID(), instId); + transmitSingleObject(inst, type, false); + } + return true; + } + else + { + return transmitSingleObject(obj, type, false); + } + } + else if (type == TYPE_OBJ_REQ) + { + return transmitSingleObject(obj, type, allInstances); + } + else if (type == TYPE_ACK) + { + if (!allInstances) + { + return transmitSingleObject(obj, type, false); + } + else + { + return false; + } + } + else + { + return false; + } +} + + +/** + * Send an object through the telemetry link. + * \param[in] obj Object handle to send + * \param[in] type Transaction type + * \return Success (true), Failure (false) + */ +bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances) +{ + qint32 length; + qint32 dataOffset; + quint16 cs = 0; + quint32 objId; + quint16 instId; + quint16 allInstId = ALL_INSTANCES; + + // Setup type and object id fields + objId = obj->getObjID(); + txBuffer[0] = type; + qToBigEndian(objId, &txBuffer[1]); + + // Setup instance ID if one is required + if ( obj->isSingleInstance() ) + { + dataOffset = 5; + } + else + { + // Check if all instances are requested + if (allInstances) + { + qToBigEndian(allInstId, &txBuffer[5]); + } + else + { + instId = obj->getInstID(); + qToBigEndian(instId, &txBuffer[5]); + } + dataOffset = 7; + } + + // Determine data length + if (type == TYPE_OBJ_REQ || type == TYPE_ACK) + { + length = 0; + } + else + { + length = obj->getNumBytes(); + } + + // Check length + if (length >= MAX_PAYLOAD_LENGTH) + { + return false; + } + + // Copy data (if any) + if (length > 0) + { + if ( !obj->pack(&txBuffer[dataOffset]) ) + { + return false; + } + } + + // Calculate checksum + cs = 0; + cs = updateChecksum(cs, txBuffer, dataOffset+length); + qToBigEndian(cs, &txBuffer[dataOffset+length]); + + // Send buffer + io->write((const char*)txBuffer, dataOffset+length+CHECKSUM_LENGTH); + + // Done + return true; +} + +/** + * Update checksum. + * TODO: Replace with CRC-16 + * \param[in] data Data buffer to update checksum on + * \param[in] length Length of buffer + * \return Updated checksum + */ +quint16 UAVTalk::updateChecksum(quint16 cs, quint8* data, qint32 length) +{ + qint32 n; + for (n = 0; n < length; ++n) + { + cs += (quint16)data[n]; + } + return cs; +} + + + + + + + + + + + + + + + + + diff --git a/ground/src/plugins/uavtalk/uavtalk.h b/ground/src/plugins/uavtalk/uavtalk.h new file mode 100644 index 000000000..da95957a4 --- /dev/null +++ b/ground/src/plugins/uavtalk/uavtalk.h @@ -0,0 +1,73 @@ +#ifndef UAVTALK_H +#define UAVTALK_H + +#include +#include +#include +#include +#include "uavobjects\uavobjectmanager.h" + +class UAVTalk: public QObject +{ + Q_OBJECT + +public: + UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr); + + qint32 sendObject(UAVObject* obj, bool acked, qint32 timeoutMs); + qint32 sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances); + +signals: + void transactionCompleted(UAVObject* obj); + +private slots: + void processInputStream(void); + +private: + // Constants + static const int TYPE_MASK = 0xFC; + static const int TYPE_VER = 0x10; + static const int TYPE_OBJ = (TYPE_VER | 0x00); + static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01); + static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02); + static const int TYPE_ACK = (TYPE_VER | 0x03); + static const int HEADER_LENGTH = 7; // type (1), object ID (4), instance ID (2, not used in single objects) + static const int CHECKSUM_LENGTH = 2; + static const int MAX_PAYLOAD_LENGTH = 256; + static const int MAX_PACKET_LENGTH = (HEADER_LENGTH+MAX_PAYLOAD_LENGTH+CHECKSUM_LENGTH); + static const quint16 ALL_INSTANCES = 0xFFFF; + + typedef enum {STATE_SYNC, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxStateType; + + // Variables + QIODevice* io; + UAVObjectManager* objMngr; + QMutex* mutex; + QSemaphore* respSema; + UAVObject* respObj; + bool respAllInstances; + quint8 rxBuffer[MAX_PACKET_LENGTH]; + quint8 txBuffer[MAX_PACKET_LENGTH]; + // Variables used by the receive state machine + quint8 rxTmpBuffer[4]; + quint8 rxType; + quint32 rxObjId; + quint16 rxInstId; + quint8 rxLength; + quint16 rxCSPacket, rxCS; + qint32 rxCount; + RxStateType rxState; + + // Methods + bool objectTransaction(UAVObject* obj, quint8 type, qint32 timeoutMs, bool allInstances); + bool processInputByte(quint8 rxbyte); + bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* data, qint32 length); + UAVObject* updateObject(quint32 objId, quint16 instId, quint8* data); + void updateAck(UAVObject* obj); + bool transmitObject(UAVObject* obj, quint8 type, bool allInstances); + bool transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances); + quint16 updateChecksum(quint16 cs, quint8* data, qint32 length); + +}; + +#endif // UAVTALK_H diff --git a/ground/src/plugins/uavtalk/uavtalk.pluginspec b/ground/src/plugins/uavtalk/uavtalk.pluginspec new file mode 100644 index 000000000..04d1ed779 --- /dev/null +++ b/ground/src/plugins/uavtalk/uavtalk.pluginspec @@ -0,0 +1,10 @@ + + The OpenPilot Project + (C) 2010 OpenPilot Project + Your License goes here + UAVTalk telemetry protocol + http://www.openpilot.org + + + + \ No newline at end of file diff --git a/ground/src/plugins/uavtalk/uavtalk.pro b/ground/src/plugins/uavtalk/uavtalk.pro new file mode 100644 index 000000000..2e767806a --- /dev/null +++ b/ground/src/plugins/uavtalk/uavtalk.pro @@ -0,0 +1,11 @@ +TEMPLATE = lib +TARGET = UAVTalk +include(../../openpilotgcsplugin.pri) +include(../../plugins/coreplugin/coreplugin.pri) +HEADERS += uavtalk.h \ + uavtalkplugin.h +SOURCES += uavtalk.cpp \ + uavtalkplugin.cpp +HEADERS += telemetry.h +SOURCES += telemetry.cpp +OTHER_FILES += uavtalk.pluginspec diff --git a/ground/src/plugins/uavtalk/uavtalkplugin.cpp b/ground/src/plugins/uavtalk/uavtalkplugin.cpp new file mode 100644 index 000000000..0379fd289 --- /dev/null +++ b/ground/src/plugins/uavtalk/uavtalkplugin.cpp @@ -0,0 +1,5 @@ +#include "uavtalkplugin.h" + +UAVTalkPlugin::UAVTalkPlugin() +{ +} diff --git a/ground/src/plugins/uavtalk/uavtalkplugin.h b/ground/src/plugins/uavtalk/uavtalkplugin.h new file mode 100644 index 000000000..066f45ad9 --- /dev/null +++ b/ground/src/plugins/uavtalk/uavtalkplugin.h @@ -0,0 +1,10 @@ +#ifndef UAVTALKPLUGIN_H +#define UAVTALKPLUGIN_H + +class UAVTalkPlugin +{ +public: + UAVTalkPlugin(); +}; + +#endif // UAVTALKPLUGIN_H