rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

OP-1122 OP-1133 exposed send/request all instances of multi instance uav objects and made related uavtalk fixes

Browse Source
This commit is contained in:
Philippe Renon 2013-12-01 19:25:44 +01:00
parent a6d0f09d21
commit 0dc334adea
7 changed files with 231 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/uavtalk/uavtalk.c
View File

@ -340,8 +340,10 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
}
if (iproc->rxPacketLength < 0xffff) {
iproc->rxPacketLength++; // update packet byte count
// update packet byte count
iproc->rxPacketLength++;
}
// Receive state machine
switch (iproc->state) {
case UAVTALK_STATE_SYNC:
@ -445,7 +447,6 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->rxCount = 0;
iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID;
break;
case UAVTALK_STATE_INSTID:
@ -472,7 +473,6 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
} else {
iproc->state = UAVTALK_STATE_CS;
}
break;
case UAVTALK_STATE_TIMESTAMP:
@ -653,11 +653,14 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle)
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1);
UAVTalkInputProcessor *iproc = &connection->iproc;
if (iproc->state != UAVTALK_STATE_COMPLETE) {
return -1;
}
receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length);
return 0;
}
@ -755,11 +758,12 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
}
break;
case UAVTALK_TYPE_OBJ_REQ:
// Check if requested object exists
if (obj && ((instId == UAVOBJ_ALL_INSTANCES) || instId < UAVObjGetNumInstances(obj))) {
// Object found, transmit it
// This OBJ message will also ack this request
// The sent object will ack the object request on the receiver side
sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj);
} else {
ret = -1;
@ -769,10 +773,18 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
}
break;
case UAVTALK_TYPE_NACK:
// Do nothing on flight side, let it time out.
// Why?
// TODO:
// The transaction takes the result code of the "semaphore taking operation" into account to determine success.
// If we give that semaphore in time, its "success" (ack received)
// If we do not give that semaphore before the timeout it will return failure.
// What would have to be done here is give the semaphore, but set a flag (for example connection->respFail=true)
// that indicates failure and then above where it checks for the result code, have it behave as if it failed
// if the explicit failure is set.
break;
case UAVTALK_TYPE_ACK:
// All instances, not allowed for ACK messages
if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
@ -782,6 +794,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
ret = -1;
}
break;
default:
ret = -1;
}
@ -833,9 +846,10 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
if (instId == UAVOBJ_ALL_INSTANCES) {
// Get number of instances
numInst = UAVObjGetNumInstances(obj);
// Send all instances
// Send all instances in reverse order
// This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received)
for (n = 0; n < numInst; ++n) {
sendSingleObject(connection, type, objId, n, obj);
sendSingleObject(connection, type, objId, numInst - n - 1, obj);
}
return 0;
} else {

24
ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp
View File

@ -179,6 +179,17 @@ void UAVObject::requestUpdate()
emit updateRequested(this);
}
/**
* Request that all instances of this object are updated with the latest values from the autopilot
* Must be called on instance zero
*/
void UAVObject::requestUpdateAll()
{
if (instID == 0) {
emit updateRequested(this, true);
}
}
/**
* Signal that the object has been updated
*/
@ -188,6 +199,19 @@ void UAVObject::updated()
emit objectUpdated(this);
}
/**
* Signal that all instance of the object have been updated
* Must be called on instance zero
*/
void UAVObject::updatedAll()
{
if (instID == 0) {
emit objectUpdatedManual(this, true);
// TODO call objectUpdated() for all instances?
//emit objectUpdated(this);
}
}
/**
* Lock mutex of this object
*/

6
ground/openpilotgcs/src/plugins/uavobjects/uavobject.h
View File

@ -144,15 +144,17 @@ public:
public slots:
void requestUpdate();
void requestUpdateAll();
void updated();
void updatedAll();
signals:
void objectUpdated(UAVObject *obj);
void objectUpdatedAuto(UAVObject *obj);
void objectUpdatedManual(UAVObject *obj);
void objectUpdatedManual(UAVObject *obj, bool all = false);
void objectUpdatedPeriodic(UAVObject *obj);
void objectUnpacked(UAVObject *obj);
void updateRequested(UAVObject *obj);
void updateRequested(UAVObject *obj, bool all = false);
void transactionCompleted(UAVObject *obj, bool success);
void newInstance(UAVObject *obj);

147
ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp
View File

@ -36,28 +36,33 @@
/**
* Constructor
*/
Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr)
Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) : objMngr(objMngr), utalk(utalk)
{
this->utalk = utalk;
this->objMngr = objMngr;
mutex = new QMutex(QMutex::Recursive);
// Process all objects in the list
// Register all objects in the list
QList< QList<UAVObject *> > objs = objMngr->getObjects();
for (int objidx = 0; objidx < objs.length(); ++objidx) {
registerObject(objs[objidx][0]); // we only need to register one instance per object type
// we only need to register one instance per object type
registerObject(objs[objidx][0]);
}
// Listen to new object creations
connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *)));
connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *)));
// Listen to transaction completions
connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool)));
// Get GCS stats object
gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr);
// Setup and start the periodic timer
timeToNextUpdateMs = 0;
updateTimer = new QTimer(this);
connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates()));
updateTimer->start(1000);
// Setup and start the stats timer
txErrors = 0;
txRetries = 0;
@ -66,7 +71,6 @@ Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr)
Telemetry::~Telemetry()
{
closeAllTransactions();
}
/**
@ -121,27 +125,36 @@ void Telemetry::setUpdatePeriod(UAVObject *obj, qint32 periodMs)
*/
void Telemetry::connectToObjectInstances(UAVObject *obj, quint32 eventMask)
{
// TODO why connect systematically to all instances?
// TODO why connect systematically to all instances?
// It is probably not needed to connect to always connect to all instances.
QList<UAVObject *> objs = objMngr->getObjectInstances(obj->getObjID());
for (int n = 0; n < objs.length(); ++n) {
// Disconnect all
objs[n]->disconnect(this);
// Connect only the selected events
if ((eventMask & EV_UNPACKED) != 0) {
connect(objs[n], SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *)));
}
if ((eventMask & EV_UPDATED) != 0) {
connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *)));
}
if ((eventMask & EV_UPDATED_MANUAL) != 0) {
connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject *)), this, SLOT(objectUpdatedManual(UAVObject *)));
}
if ((eventMask & EV_UPDATED_PERIODIC) != 0) {
connect(objs[n], SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *)));
}
if ((eventMask & EV_UPDATE_REQ) != 0) {
connect(objs[n], SIGNAL(updateRequested(UAVObject *)), this, SLOT(updateRequested(UAVObject *)));
}
connectToObject(objs[n], eventMask);
}
}
/**
* Connect to all instances of an object depending on the event mask specified
*/
void Telemetry::connectToObject(UAVObject *obj, quint32 eventMask)
{
// Disconnect all
obj->disconnect(this);
// Connect only the selected events
if ((eventMask & EV_UNPACKED) != 0) {
connect(obj, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *)));
}
if ((eventMask & EV_UPDATED) != 0) {
connect(obj, SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *)));
}
if ((eventMask & EV_UPDATED_MANUAL) != 0) {
connect(obj, SIGNAL(objectUpdatedManual(UAVObject *, bool)), this, SLOT(objectUpdatedManual(UAVObject *, bool)));
}
if ((eventMask & EV_UPDATED_PERIODIC) != 0) {
connect(obj, SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *)));
}
if ((eventMask & EV_UPDATE_REQ) != 0) {
connect(obj, SIGNAL(updateRequested(UAVObject *, bool)), this, SLOT(updateRequested(UAVObject *, bool)));
}
}
@ -160,19 +173,21 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
if (updateMode == UAVObject::UPDATEMODE_PERIODIC) {
// Set update period
setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
// Connect signals for all instances
// Connect signals
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
// we also need to act on remote updates (unpack events)
eventMask |= EV_UNPACKED;
}
connectToObjectInstances(obj, eventMask);
} else if (updateMode == UAVObject::UPDATEMODE_ONCHANGE) {
// Set update period
setUpdatePeriod(obj, 0);
// Connect signals for all instances
// Connect signals
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
// we also need to act on remote updates (unpack events)
eventMask |= EV_UNPACKED;
}
connectToObjectInstances(obj, eventMask);
} else if (updateMode == UAVObject::UPDATEMODE_THROTTLED) {
@ -182,24 +197,26 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
if (eventType == EV_NONE) {
setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
}
// Connect signals for all instances
// Connect signals
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
} else {
// Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
// Connect signals for all instances
// Connect signals
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
}
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
// we also need to act on remote updates (unpack events)
eventMask |= EV_UNPACKED;
}
connectToObjectInstances(obj, eventMask);
} else if (updateMode == UAVObject::UPDATEMODE_MANUAL) {
// Set update period
setUpdatePeriod(obj, 0);
// Connect signals for all instances
// Connect signals
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
// we also need to act on remote updates (unpack events)
eventMask |= EV_UNPACKED;
}
connectToObjectInstances(obj, eventMask);
}
@ -242,7 +259,7 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
// Check if more retries are pending
if (transInfo->retriesRemaining > 0) {
#ifdef VERBOSE_TELEMETRY
qDebug() << QString("Telemetry - transaction timed out for object %1, retrying...").arg(transInfo->obj->toStringBrief());
qDebug().nospace() << "Telemetry - transaction timed out for object " << transInfo->obj->toStringBrief() << ", retrying...";
#endif
--transInfo->retriesRemaining;
processObjectTransaction(transInfo);
@ -253,7 +270,7 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
// Terminate transaction
utalk->cancelTransaction(transInfo->obj);
// Send signal
qWarning() << QString("Telemetry - !!! transaction timed out for object %1").arg(transInfo->obj->toStringBrief());
qWarning().nospace() << "Telemetry - !!! transaction timed out for object " << transInfo->obj->toStringBrief();
transInfo->obj->emitTransactionCompleted(false);
// Remove this transaction as it's complete.
// FIXME : also remove transaction from UAVTalk transaction map
@ -273,12 +290,12 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo)
// Initiate transaction
if (transInfo->objRequest) {
#ifdef VERBOSE_TELEMETRY
qDebug() << QString("Telemetry - sending object request for %1 - %2").arg(transInfo->obj->toStringBrief()).arg(transInfo->acked ? "acked" : "");
qDebug().nospace() << "Telemetry - sending object request for " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : "");
#endif
utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances);
} else {
#ifdef VERBOSE_TELEMETRY
qDebug() << QString("Telemetry - sending object %1 - %2").arg(transInfo->obj->toStringBrief()).arg(transInfo->acked ? "acked" : "");
qDebug().nospace() << "Telemetry - sending object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : "");
#endif
utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances);
}
@ -308,7 +325,7 @@ void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allIn
objPriorityQueue.enqueue(objInfo);
} else {
++txErrors;
qWarning() << QString("Telemetry - !!! priority event queue is full, event lost (%1)").arg(obj->toStringBrief());
qWarning().nospace() << "Telemetry - !!! priority event queue is full, event lost " << obj->toStringBrief();
obj->emitTransactionCompleted(false);
}
} else {
@ -316,7 +333,7 @@ void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allIn
objQueue.enqueue(objInfo);
} else {
++txErrors;
qWarning() << QString("Telemetry - !!! event queue is full, event lost (%1)").arg(obj->toStringBrief());
qWarning().nospace() << "Telemetry - !!! event queue is full, event lost " << obj->toStringBrief();
obj->emitTransactionCompleted(false);
}
}
@ -356,8 +373,13 @@ void Telemetry::processObjectQueue()
UAVObject::Metadata metadata = objInfo.obj->getMetadata();
UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata);
if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC) || (updateMode != UAVObject::UPDATEMODE_THROTTLED))) {
// Check if a transaction for that object already exists
// It is allowed to have multiple transaction on the same object ID provided that the instance IDs are different
// If an "all instances" transaction is running, then it is not allowed to start another transaction with same object ID
// If a single instance transaction is running, then starting an "all instance" transaction is not allowed
// TODO make the above logic a reality...
if (findTransaction(objInfo.obj)) {
qWarning() << QString("Telemetry - !!! Making request for an object: %1 for which a request is already in progress").arg(objInfo.obj->toStringBrief());
qWarning() << "Telemetry - !!! Making request for a object " << objInfo.obj->toStringBrief() << " for which a request is already in progress";
objInfo.obj->emitTransactionCompleted(false);
return;
}
@ -374,7 +396,7 @@ void Telemetry::processObjectQueue()
}
transInfo->telem = this;
// Insert the transaction into the transaction map.
startTransaction(transInfo);
openTransaction(transInfo);
processObjectTransaction(transInfo);
}
@ -491,11 +513,12 @@ void Telemetry::objectUpdatedAuto(UAVObject *obj)
processObjectUpdates(obj, EV_UPDATED, false, true);
}
void Telemetry::objectUpdatedManual(UAVObject *obj)
void Telemetry::objectUpdatedManual(UAVObject *obj, bool all)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UPDATED_MANUAL, false, true);
bool allInstances = obj->isSingleInstance() ? false : all;
processObjectUpdates(obj, EV_UPDATED_MANUAL, allInstances, true);
}
void Telemetry::objectUpdatedPeriodic(UAVObject *obj)
@ -512,10 +535,12 @@ void Telemetry::objectUnpacked(UAVObject *obj)
processObjectUpdates(obj, EV_UNPACKED, false, true);
}
void Telemetry::updateRequested(UAVObject *obj)
void Telemetry::updateRequested(UAVObject *obj, bool all)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UPDATE_REQ, false, true);
bool allInstances = obj->isSingleInstance() ? false : all;
processObjectUpdates(obj, EV_UPDATE_REQ, allInstances, true);
}
void Telemetry::newObject(UAVObject *obj)
@ -534,29 +559,43 @@ void Telemetry::newInstance(UAVObject *obj)
ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj)
{
quint32 objId = obj->getObjID();
quint16 instId = obj->getInstID();
// Lookup the transaction in the transaction map
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(obj->getObjID());
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
if (objTransactions != NULL) {
return objTransactions->value(obj->getInstID());
ObjectTransactionInfo *trans = objTransactions->value(instId);
if (trans == NULL) {
// see if there is an ALL_INSTANCES transaction
trans = objTransactions->value(UAVTalk::ALL_INSTANCES);
}
return trans;
}
return NULL;
}
void Telemetry::startTransaction(ObjectTransactionInfo *trans)
void Telemetry::openTransaction(ObjectTransactionInfo *trans)
{
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(trans->obj->getObjID());
quint32 objId = trans->obj->getObjID();
quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID();
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
if (objTransactions == NULL) {
objTransactions = new QMap<quint32, ObjectTransactionInfo *>();
transMap.insert(trans->obj->getObjID(), objTransactions);
transMap.insert(objId, objTransactions);
}
objTransactions->insert(trans->obj->getInstID(), trans);
objTransactions->insert(instId, trans);
}
void Telemetry::closeTransaction(ObjectTransactionInfo *trans)
{
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(trans->obj->getObjID());
quint32 objId = trans->obj->getObjID();
quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID();
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
if (objTransactions != NULL) {
objTransactions->remove(trans->obj->getInstID());
objTransactions->remove(instId);
// Keep the map even if it is empty
// There are at most 100 different object IDs...
}

11
ground/openpilotgcs/src/plugins/uavtalk/telemetry.h
View File

@ -95,8 +95,8 @@ private:
EV_UNPACKED = 0x01, /** Object data updated by unpacking */
EV_UPDATED = 0x02, /** Object data updated by changing the data structure */
EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */
EV_UPDATED_PERIODIC = 0x8, /** Object update event generated by timer */
EV_UPDATE_REQ = 0x010 /** Request to update object data */
EV_UPDATED_PERIODIC = 0x08, /** Object update event generated by timer */
EV_UPDATE_REQ = 0x10 /** Request to update object data */
} EventMask;
typedef struct {
@ -131,22 +131,23 @@ private:
void addObject(UAVObject *obj);
void setUpdatePeriod(UAVObject *obj, qint32 periodMs);
void connectToObjectInstances(UAVObject *obj, quint32 eventMask);
void connectToObject(UAVObject *obj, quint32 eventMask);
void updateObject(UAVObject *obj, quint32 eventMask);
void processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority);
void processObjectTransaction(ObjectTransactionInfo *transInfo);
void processObjectQueue();
ObjectTransactionInfo *findTransaction(UAVObject *obj);
void startTransaction(ObjectTransactionInfo *trans);
void openTransaction(ObjectTransactionInfo *trans);
void closeTransaction(ObjectTransactionInfo *trans);
void closeAllTransactions();
private slots:
void objectUpdatedAuto(UAVObject *obj);
void objectUpdatedManual(UAVObject *obj);
void objectUpdatedManual(UAVObject *obj, bool all = false);
void objectUpdatedPeriodic(UAVObject *obj);
void objectUnpacked(UAVObject *obj);
void updateRequested(UAVObject *obj);
void updateRequested(UAVObject *obj, bool all = false);
void newObject(UAVObject *obj);
void newInstance(UAVObject *obj);
void processPeriodicUpdates();

138
ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp
View File

@ -29,6 +29,7 @@
#include <QDebug>
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h>
// #define UAVTALK_DEBUG
#ifdef UAVTALK_DEBUG
#define UAVTALK_QXTLOG_DEBUG(args ...)
@ -36,6 +37,8 @@
#define UAVTALK_QXTLOG_DEBUG(args ...)
#endif // UAVTALK_DEBUG
//#define VERBOSE_FILTER(objId) if (objId == 0xD23852DC)
#define SYNC_VAL 0x3C
const quint8 UAVTalk::crc_table[256] = {
@ -205,7 +208,6 @@ void UAVTalk::cancelTransaction(UAVObject *obj)
Transaction *trans = findTransaction(obj->getObjID(), obj->getInstID());
if (trans != NULL) {
closeTransaction(trans);
delete trans;
}
}
@ -226,11 +228,7 @@ bool UAVTalk::objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVO
// transactions of TYPE_OBJ_REQ are acked by the response
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) {
if (transmitObject(type, objId, instId, obj)) {
Transaction *trans = new Transaction();
trans->respType = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK;
trans->respObjId = objId;
trans->respInstId = instId;
startTransaction(trans);
openTransaction(type, objId, instId);
return true;
} else {
return false;
@ -507,7 +505,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
// Get object and update its data
obj = updateObject(objId, instId, data);
#ifdef VERBOSE_UAVTALK
qDebug() << "UAVTalk - received object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
// Check if this object acks a pending OBJ_REQ message
@ -521,13 +519,14 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
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);
#ifdef VERBOSE_UAVTALK
qDebug() << "UAVTalk - received object (acked)" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object (acked)" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
// Object updated or created, transmit ACK
@ -543,6 +542,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
transmitObject(TYPE_NACK, objId, instId, NULL);
}
break;
case TYPE_OBJ_REQ:
// Check if requested object exists
if (allInstances) {
@ -552,10 +552,11 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
obj = objMngr->getObject(objId, instId);
}
#ifdef VERBOSE_UAVTALK
qDebug() << "UAVTalk - received object request" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object request" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
// Object found, transmit it
// The sent object will ack the object request on the receiver side
transmitObject(TYPE_OBJ, objId, instId, obj);
} else {
error = true;
@ -565,29 +566,14 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
transmitObject(TYPE_NACK, objId, instId, NULL);
}
break;
case TYPE_NACK:
// All instances, not allowed for NACK messages
if (!allInstances) {
// Get object
obj = objMngr->getObject(objId, instId);
#ifdef VERBOSE_UAVTALK
qDebug() << "UAVTalk - received nack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
// Check if a NACK is pending
updateNack(type, objId, instId, obj);
} else {
error = true;
}
}
break;
case TYPE_ACK:
// All instances, not allowed for ACK messages
if (!allInstances) {
// Get object
obj = objMngr->getObject(objId, instId);
#ifdef VERBOSE_UAVTALK
qDebug() << "UAVTalk - received ack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received ack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
// Check if an ACK is pending
@ -597,6 +583,24 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
}
}
break;
case TYPE_NACK:
// All instances, not allowed for NACK messages
if (!allInstances) {
// Get object
obj = objMngr->getObject(objId, instId);
#ifdef VERBOSE_UAVTALK
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received nack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
// Check if a NACK is pending
updateNack(objId, instId, obj);
} else {
error = true;
}
}
break;
default:
error = true;
}
@ -620,22 +624,25 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data)
// If the instance does not exist create it
if (obj == NULL) {
// Get the object type
UAVObject *tobj = objMngr->getObject(objId);
if (tobj == NULL) {
UAVObject *typeObj = objMngr->getObject(objId);
if (typeObj == NULL) {
qWarning() << "UAVTalk - Failed to get object, object ID :" << objId;
return NULL;
}
// Make sure this is a data object
UAVDataObject *dobj = dynamic_cast<UAVDataObject *>(tobj);
if (dobj == NULL) {
UAVDataObject *dataObj = dynamic_cast<UAVDataObject *>(typeObj);
if (dataObj == NULL) {
qWarning() << "UAVTalk - Object is not a data object, object ID :" << objId;
return NULL;
}
// Create a new instance, unpack and register
UAVDataObject *instobj = dobj->clone(instId);
if (!objMngr->registerObject(instobj)) {
UAVDataObject *instObj = dataObj->clone(instId);
if (!objMngr->registerObject(instObj)) {
qWarning() << "UAVTalk - Failed to register object " << instObj->toStringBrief();
return NULL;
}
instobj->unpack(data);
return instobj;
instObj->unpack(data);
return instObj;
} else {
// Unpack data into object instance
obj->unpack(data);
@ -643,23 +650,6 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data)
}
}
/**
* Check if a transaction is pending and if yes complete it.
*/
void UAVTalk::updateNack(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
{
Q_ASSERT(obj);
if (!obj) {
return;
}
Transaction *trans = findTransaction(objId, instId);
if (trans) {
closeTransaction(trans);
delete trans;
emit transactionCompleted(obj, false);
}
}
/**
* Check if a transaction is pending and if yes complete it.
*/
@ -671,9 +661,26 @@ void UAVTalk::updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *o
}
Transaction *trans = findTransaction(objId, instId);
if (trans && trans->respType == type) {
if (trans->respInstId != ALL_INSTANCES || instId == 0) {
closeTransaction(trans);
emit transactionCompleted(obj, true);
}
}
}
/**
* Check if a transaction is pending and if yes complete it.
*/
void UAVTalk::updateNack(quint32 objId, quint16 instId, UAVObject *obj)
{
Q_ASSERT(obj);
if (!obj) {
return;
}
Transaction *trans = findTransaction(objId, instId);
if (trans) {
closeTransaction(trans);
delete trans;
emit transactionCompleted(obj, true);
emit transactionCompleted(obj, false);
}
}
@ -700,10 +707,12 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje
if (allInstances) {
// Get number of instances
quint32 numInst = objMngr->getNumInstances(objId);
// Send all instances
// Send all instances in reverse order
// This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received)
for (quint32 n = 0; n < numInst; ++n) {
UAVObject *o = objMngr->getObject(objId, n);
transmitSingleObject(type, objId, n, o);
quint32 i = numInst - n - 1;
UAVObject *o = objMngr->getObject(objId, i);
transmitSingleObject(type, objId, i, o);
}
return true;
} else {
@ -736,7 +745,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U
qint32 dataOffset;
#ifdef VERBOSE_UAVTALK
qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
// IMPORTANT : obj can be null (when type is NACK for example)
@ -833,13 +842,23 @@ UAVTalk::Transaction *UAVTalk::findTransaction(quint32 objId, quint16 instId)
// Lookup the transaction in the transaction map
QMap<quint32, Transaction *> *objTransactions = transMap.value(objId);
if (objTransactions != NULL) {
return objTransactions->value(instId);
Transaction *trans = objTransactions->value(instId);
if (trans == NULL) {
// see if there is an ALL_INSTANCES transaction
trans = objTransactions->value(ALL_INSTANCES);
}
return trans;
}
return NULL;
}
void UAVTalk::startTransaction(Transaction *trans)
void UAVTalk::openTransaction(quint8 type, quint32 objId, quint16 instId)
{
Transaction *trans = new Transaction();
trans->respType = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK;
trans->respObjId = objId;
trans->respInstId = instId;
QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
if (objTransactions == NULL) {
objTransactions = new QMap<quint32, Transaction *>();
@ -855,6 +874,7 @@ void UAVTalk::closeTransaction(Transaction *trans)
objTransactions->remove(trans->respInstId);
// Keep the map even if it is empty
// There are at most 100 different object IDs...
delete trans;
}
}

8
ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h
View File

@ -41,6 +41,8 @@ class UAVTALK_EXPORT UAVTalk : public QObject {
Q_OBJECT
public:
static const quint16 ALL_INSTANCES = 0xFFFF;
typedef struct {
quint32 txBytes;
quint32 rxBytes;
@ -93,8 +95,6 @@ private:
static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH);
static const quint16 ALL_INSTANCES = 0xFFFF;
static const quint8 INSTANCE_LENGTH = 2;
static const int TX_BUFFER_SIZE = 2 * 1024;
@ -135,14 +135,14 @@ private:
bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length);
UAVObject *updateObject(quint32 objId, quint16 instId, quint8 *data);
void updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
void updateNack(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
void updateNack(quint32 objId, quint16 instId, UAVObject *obj);
bool transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
bool transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
quint8 updateCRC(quint8 crc, const quint8 data);
quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length);
Transaction *findTransaction(quint32 objId, quint16 instId);
void startTransaction(Transaction *trans);
void openTransaction(quint8 type, quint32 objId, quint16 instId);
void closeTransaction(Transaction *trans);
void closeAllTransactions();
};