rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +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) { if (iproc->rxPacketLength < 0xffff) {
iproc->rxPacketLength++; // update packet byte count // update packet byte count
iproc->rxPacketLength++;
} }
// Receive state machine // Receive state machine
switch (iproc->state) { switch (iproc->state) {
case UAVTALK_STATE_SYNC: case UAVTALK_STATE_SYNC:
@ -445,7 +447,6 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->rxCount = 0; iproc->rxCount = 0;
iproc->instId = 0; iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID; iproc->state = UAVTALK_STATE_INSTID;
break; break;
case UAVTALK_STATE_INSTID: case UAVTALK_STATE_INSTID:
@ -472,7 +473,6 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
} else { } else {
iproc->state = UAVTALK_STATE_CS; iproc->state = UAVTALK_STATE_CS;
} }
break; break;
case UAVTALK_STATE_TIMESTAMP: case UAVTALK_STATE_TIMESTAMP:
@ -653,11 +653,14 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle)
UAVTalkConnectionData *connection; UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1); CHECKCONHANDLE(connectionHandle, connection, return -1);
UAVTalkInputProcessor *iproc = &connection->iproc; UAVTalkInputProcessor *iproc = &connection->iproc;
if (iproc->state != UAVTALK_STATE_COMPLETE) { if (iproc->state != UAVTALK_STATE_COMPLETE) {
return -1; return -1;
} }
receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length);
return 0; return 0;
} }
@ -755,11 +758,12 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL); sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
} }
break; break;
case UAVTALK_TYPE_OBJ_REQ: case UAVTALK_TYPE_OBJ_REQ:
// Check if requested object exists // Check if requested object exists
if (obj && ((instId == UAVOBJ_ALL_INSTANCES) || instId < UAVObjGetNumInstances(obj))) { if (obj && ((instId == UAVOBJ_ALL_INSTANCES) || instId < UAVObjGetNumInstances(obj))) {
// Object found, transmit it // 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); sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj);
} else { } else {
ret = -1; ret = -1;
@ -769,10 +773,18 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL); sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
} }
break; break;
case UAVTALK_TYPE_NACK: case UAVTALK_TYPE_NACK:
// Do nothing on flight side, let it time out. // 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; break;
case UAVTALK_TYPE_ACK: case UAVTALK_TYPE_ACK:
// All instances, not allowed for ACK messages // All instances, not allowed for ACK messages
if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
@ -782,6 +794,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
ret = -1; ret = -1;
} }
break; break;
default: default:
ret = -1; ret = -1;
} }
@ -833,9 +846,10 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
if (instId == UAVOBJ_ALL_INSTANCES) { if (instId == UAVOBJ_ALL_INSTANCES) {
// Get number of instances // Get number of instances
numInst = UAVObjGetNumInstances(obj); 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) { for (n = 0; n < numInst; ++n) {
sendSingleObject(connection, type, objId, n, obj); sendSingleObject(connection, type, objId, numInst - n - 1, obj);
} }
return 0; return 0;
} else { } else {

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

@ -179,6 +179,17 @@ void UAVObject::requestUpdate()
emit updateRequested(this); 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 * Signal that the object has been updated
*/ */
@ -188,6 +199,19 @@ void UAVObject::updated()
emit objectUpdated(this); 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 * Lock mutex of this object
*/ */

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

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

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

@ -36,28 +36,33 @@
/** /**
* Constructor * 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); mutex = new QMutex(QMutex::Recursive);
// Process all objects in the list
// Register all objects in the list
QList< QList<UAVObject *> > objs = objMngr->getObjects(); QList< QList<UAVObject *> > objs = objMngr->getObjects();
for (int objidx = 0; objidx < objs.length(); ++objidx) { 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 // Listen to new object creations
connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *))); connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *)));
connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *))); connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *)));
// Listen to transaction completions // Listen to transaction completions
connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool))); connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool)));
// Get GCS stats object // Get GCS stats object
gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr); gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr);
// Setup and start the periodic timer // Setup and start the periodic timer
timeToNextUpdateMs = 0; timeToNextUpdateMs = 0;
updateTimer = new QTimer(this); updateTimer = new QTimer(this);
connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates())); connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates()));
updateTimer->start(1000); updateTimer->start(1000);
// Setup and start the stats timer // Setup and start the stats timer
txErrors = 0; txErrors = 0;
txRetries = 0; txRetries = 0;
@ -66,7 +71,6 @@ Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr)
Telemetry::~Telemetry() Telemetry::~Telemetry()
{ {
closeAllTransactions(); closeAllTransactions();
} }
/** /**
@ -121,27 +125,36 @@ void Telemetry::setUpdatePeriod(UAVObject *obj, qint32 periodMs)
*/ */
void Telemetry::connectToObjectInstances(UAVObject *obj, quint32 eventMask) 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()); QList<UAVObject *> objs = objMngr->getObjectInstances(obj->getObjID());
for (int n = 0; n < objs.length(); ++n) { for (int n = 0; n < objs.length(); ++n) {
// Disconnect all connectToObject(objs[n], eventMask);
objs[n]->disconnect(this); }
// Connect only the selected events }
if ((eventMask & EV_UNPACKED) != 0) {
connect(objs[n], SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *))); /**
} * Connect to all instances of an object depending on the event mask specified
if ((eventMask & EV_UPDATED) != 0) { */
connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *))); void Telemetry::connectToObject(UAVObject *obj, quint32 eventMask)
} {
if ((eventMask & EV_UPDATED_MANUAL) != 0) { // Disconnect all
connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject *)), this, SLOT(objectUpdatedManual(UAVObject *))); obj->disconnect(this);
} // Connect only the selected events
if ((eventMask & EV_UPDATED_PERIODIC) != 0) { if ((eventMask & EV_UNPACKED) != 0) {
connect(objs[n], SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *))); connect(obj, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *)));
} }
if ((eventMask & EV_UPDATE_REQ) != 0) { if ((eventMask & EV_UPDATED) != 0) {
connect(objs[n], SIGNAL(updateRequested(UAVObject *)), this, SLOT(updateRequested(UAVObject *))); 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) { if (updateMode == UAVObject::UPDATEMODE_PERIODIC) {
// Set update period // Set update period
setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
// Connect signals for all instances // Connect signals
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC; eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) { 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); connectToObjectInstances(obj, eventMask);
} else if (updateMode == UAVObject::UPDATEMODE_ONCHANGE) { } else if (updateMode == UAVObject::UPDATEMODE_ONCHANGE) {
// Set update period // Set update period
setUpdatePeriod(obj, 0); setUpdatePeriod(obj, 0);
// Connect signals for all instances // Connect signals
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) { 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); connectToObjectInstances(obj, eventMask);
} else if (updateMode == UAVObject::UPDATEMODE_THROTTLED) { } else if (updateMode == UAVObject::UPDATEMODE_THROTTLED) {
@ -182,24 +197,26 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
if (eventType == EV_NONE) { if (eventType == EV_NONE) {
setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
} }
// Connect signals for all instances // Connect signals
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC; eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
} else { } else {
// Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates // 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; eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
} }
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) { 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); connectToObjectInstances(obj, eventMask);
} else if (updateMode == UAVObject::UPDATEMODE_MANUAL) { } else if (updateMode == UAVObject::UPDATEMODE_MANUAL) {
// Set update period // Set update period
setUpdatePeriod(obj, 0); setUpdatePeriod(obj, 0);
// Connect signals for all instances // Connect signals
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) { 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); connectToObjectInstances(obj, eventMask);
} }
@ -242,7 +259,7 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
// Check if more retries are pending // Check if more retries are pending
if (transInfo->retriesRemaining > 0) { if (transInfo->retriesRemaining > 0) {
#ifdef VERBOSE_TELEMETRY #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 #endif
--transInfo->retriesRemaining; --transInfo->retriesRemaining;
processObjectTransaction(transInfo); processObjectTransaction(transInfo);
@ -253,7 +270,7 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
// Terminate transaction // Terminate transaction
utalk->cancelTransaction(transInfo->obj); utalk->cancelTransaction(transInfo->obj);
// Send signal // 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); transInfo->obj->emitTransactionCompleted(false);
// Remove this transaction as it's complete. // Remove this transaction as it's complete.
// FIXME : also remove transaction from UAVTalk transaction map // FIXME : also remove transaction from UAVTalk transaction map
@ -273,12 +290,12 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo)
// Initiate transaction // Initiate transaction
if (transInfo->objRequest) { if (transInfo->objRequest) {
#ifdef VERBOSE_TELEMETRY #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 #endif
utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances); utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances);
} else { } else {
#ifdef VERBOSE_TELEMETRY #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 #endif
utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances); utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances);
} }
@ -308,7 +325,7 @@ void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allIn
objPriorityQueue.enqueue(objInfo); objPriorityQueue.enqueue(objInfo);
} else { } else {
++txErrors; ++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); obj->emitTransactionCompleted(false);
} }
} else { } else {
@ -316,7 +333,7 @@ void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allIn
objQueue.enqueue(objInfo); objQueue.enqueue(objInfo);
} else { } else {
++txErrors; ++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); obj->emitTransactionCompleted(false);
} }
} }
@ -356,8 +373,13 @@ void Telemetry::processObjectQueue()
UAVObject::Metadata metadata = objInfo.obj->getMetadata(); UAVObject::Metadata metadata = objInfo.obj->getMetadata();
UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata); UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata);
if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC) || (updateMode != UAVObject::UPDATEMODE_THROTTLED))) { 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)) { 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); objInfo.obj->emitTransactionCompleted(false);
return; return;
} }
@ -374,7 +396,7 @@ void Telemetry::processObjectQueue()
} }
transInfo->telem = this; transInfo->telem = this;
// Insert the transaction into the transaction map. // Insert the transaction into the transaction map.
startTransaction(transInfo); openTransaction(transInfo);
processObjectTransaction(transInfo); processObjectTransaction(transInfo);
} }
@ -491,11 +513,12 @@ void Telemetry::objectUpdatedAuto(UAVObject *obj)
processObjectUpdates(obj, EV_UPDATED, false, true); processObjectUpdates(obj, EV_UPDATED, false, true);
} }
void Telemetry::objectUpdatedManual(UAVObject *obj) void Telemetry::objectUpdatedManual(UAVObject *obj, bool all)
{ {
QMutexLocker locker(mutex); 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) void Telemetry::objectUpdatedPeriodic(UAVObject *obj)
@ -512,10 +535,12 @@ void Telemetry::objectUnpacked(UAVObject *obj)
processObjectUpdates(obj, EV_UNPACKED, false, true); processObjectUpdates(obj, EV_UNPACKED, false, true);
} }
void Telemetry::updateRequested(UAVObject *obj) void Telemetry::updateRequested(UAVObject *obj, bool all)
{ {
QMutexLocker locker(mutex); 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) void Telemetry::newObject(UAVObject *obj)
@ -534,29 +559,43 @@ void Telemetry::newInstance(UAVObject *obj)
ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj) ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj)
{ {
quint32 objId = obj->getObjID();
quint16 instId = obj->getInstID();
// Lookup the transaction in the transaction map // 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) { 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; 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) { if (objTransactions == NULL) {
objTransactions = new QMap<quint32, ObjectTransactionInfo *>(); 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) 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) { if (objTransactions != NULL) {
objTransactions->remove(trans->obj->getInstID()); objTransactions->remove(instId);
// Keep the map even if it is empty // Keep the map even if it is empty
// There are at most 100 different object IDs... // 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_UNPACKED = 0x01, /** Object data updated by unpacking */
EV_UPDATED = 0x02, /** Object data updated by changing the data structure */ EV_UPDATED = 0x02, /** Object data updated by changing the data structure */
EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */ EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */
EV_UPDATED_PERIODIC = 0x8, /** Object update event generated by timer */ EV_UPDATED_PERIODIC = 0x08, /** Object update event generated by timer */
EV_UPDATE_REQ = 0x010 /** Request to update object data */ EV_UPDATE_REQ = 0x10 /** Request to update object data */
} EventMask; } EventMask;
typedef struct { typedef struct {
@ -131,22 +131,23 @@ private:
void addObject(UAVObject *obj); void addObject(UAVObject *obj);
void setUpdatePeriod(UAVObject *obj, qint32 periodMs); void setUpdatePeriod(UAVObject *obj, qint32 periodMs);
void connectToObjectInstances(UAVObject *obj, quint32 eventMask); void connectToObjectInstances(UAVObject *obj, quint32 eventMask);
void connectToObject(UAVObject *obj, quint32 eventMask);
void updateObject(UAVObject *obj, quint32 eventMask); void updateObject(UAVObject *obj, quint32 eventMask);
void processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority); void processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority);
void processObjectTransaction(ObjectTransactionInfo *transInfo); void processObjectTransaction(ObjectTransactionInfo *transInfo);
void processObjectQueue(); void processObjectQueue();
ObjectTransactionInfo *findTransaction(UAVObject *obj); ObjectTransactionInfo *findTransaction(UAVObject *obj);
void startTransaction(ObjectTransactionInfo *trans); void openTransaction(ObjectTransactionInfo *trans);
void closeTransaction(ObjectTransactionInfo *trans); void closeTransaction(ObjectTransactionInfo *trans);
void closeAllTransactions(); void closeAllTransactions();
private slots: private slots:
void objectUpdatedAuto(UAVObject *obj); void objectUpdatedAuto(UAVObject *obj);
void objectUpdatedManual(UAVObject *obj); void objectUpdatedManual(UAVObject *obj, bool all = false);
void objectUpdatedPeriodic(UAVObject *obj); void objectUpdatedPeriodic(UAVObject *obj);
void objectUnpacked(UAVObject *obj); void objectUnpacked(UAVObject *obj);
void updateRequested(UAVObject *obj); void updateRequested(UAVObject *obj, bool all = false);
void newObject(UAVObject *obj); void newObject(UAVObject *obj);
void newInstance(UAVObject *obj); void newInstance(UAVObject *obj);
void processPeriodicUpdates(); void processPeriodicUpdates();

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

@ -29,6 +29,7 @@
#include <QDebug> #include <QDebug>
#include <extensionsystem/pluginmanager.h> #include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h> #include <coreplugin/generalsettings.h>
// #define UAVTALK_DEBUG // #define UAVTALK_DEBUG
#ifdef UAVTALK_DEBUG #ifdef UAVTALK_DEBUG
#define UAVTALK_QXTLOG_DEBUG(args ...) #define UAVTALK_QXTLOG_DEBUG(args ...)
@ -36,6 +37,8 @@
#define UAVTALK_QXTLOG_DEBUG(args ...) #define UAVTALK_QXTLOG_DEBUG(args ...)
#endif // UAVTALK_DEBUG #endif // UAVTALK_DEBUG
//#define VERBOSE_FILTER(objId) if (objId == 0xD23852DC)
#define SYNC_VAL 0x3C #define SYNC_VAL 0x3C
const quint8 UAVTalk::crc_table[256] = { const quint8 UAVTalk::crc_table[256] = {
@ -205,7 +208,6 @@ void UAVTalk::cancelTransaction(UAVObject *obj)
Transaction *trans = findTransaction(obj->getObjID(), obj->getInstID()); Transaction *trans = findTransaction(obj->getObjID(), obj->getInstID());
if (trans != NULL) { if (trans != NULL) {
closeTransaction(trans); 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 // transactions of TYPE_OBJ_REQ are acked by the response
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) {
if (transmitObject(type, objId, instId, obj)) { if (transmitObject(type, objId, instId, obj)) {
Transaction *trans = new Transaction(); openTransaction(type, objId, instId);
trans->respType = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK;
trans->respObjId = objId;
trans->respInstId = instId;
startTransaction(trans);
return true; return true;
} else { } else {
return false; return false;
@ -507,7 +505,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
// Get object and update its data // Get object and update its data
obj = updateObject(objId, instId, data); obj = updateObject(objId, instId, data);
#ifdef VERBOSE_UAVTALK #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 #endif
if (obj != NULL) { if (obj != NULL) {
// Check if this object acks a pending OBJ_REQ message // 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; error = true;
} }
break; break;
case TYPE_OBJ_ACK: case TYPE_OBJ_ACK:
// All instances, not allowed for OBJ_ACK messages // All instances, not allowed for OBJ_ACK messages
if (!allInstances) { if (!allInstances) {
// Get object and update its data // Get object and update its data
obj = updateObject(objId, instId, data); obj = updateObject(objId, instId, data);
#ifdef VERBOSE_UAVTALK #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 #endif
if (obj != NULL) { if (obj != NULL) {
// Object updated or created, transmit ACK // 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); transmitObject(TYPE_NACK, objId, instId, NULL);
} }
break; break;
case TYPE_OBJ_REQ: case TYPE_OBJ_REQ:
// Check if requested object exists // Check if requested object exists
if (allInstances) { if (allInstances) {
@ -552,10 +552,11 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
obj = objMngr->getObject(objId, instId); obj = objMngr->getObject(objId, instId);
} }
#ifdef VERBOSE_UAVTALK #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 #endif
if (obj != NULL) { if (obj != NULL) {
// Object found, transmit it // Object found, transmit it
// The sent object will ack the object request on the receiver side
transmitObject(TYPE_OBJ, objId, instId, obj); transmitObject(TYPE_OBJ, objId, instId, obj);
} else { } else {
error = true; error = true;
@ -565,29 +566,14 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
transmitObject(TYPE_NACK, objId, instId, NULL); transmitObject(TYPE_NACK, objId, instId, NULL);
} }
break; 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: case TYPE_ACK:
// All instances, not allowed for ACK messages // All instances, not allowed for ACK messages
if (!allInstances) { if (!allInstances) {
// Get object // Get object
obj = objMngr->getObject(objId, instId); obj = objMngr->getObject(objId, instId);
#ifdef VERBOSE_UAVTALK #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 #endif
if (obj != NULL) { if (obj != NULL) {
// Check if an ACK is pending // Check if an ACK is pending
@ -597,6 +583,24 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
} }
} }
break; 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: default:
error = true; error = true;
} }
@ -620,22 +624,25 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data)
// If the instance does not exist create it // If the instance does not exist create it
if (obj == NULL) { if (obj == NULL) {
// Get the object type // Get the object type
UAVObject *tobj = objMngr->getObject(objId); UAVObject *typeObj = objMngr->getObject(objId);
if (tobj == NULL) { if (typeObj == NULL) {
qWarning() << "UAVTalk - Failed to get object, object ID :" << objId;
return NULL; return NULL;
} }
// Make sure this is a data object // Make sure this is a data object
UAVDataObject *dobj = dynamic_cast<UAVDataObject *>(tobj); UAVDataObject *dataObj = dynamic_cast<UAVDataObject *>(typeObj);
if (dobj == NULL) { if (dataObj == NULL) {
qWarning() << "UAVTalk - Object is not a data object, object ID :" << objId;
return NULL; return NULL;
} }
// Create a new instance, unpack and register // Create a new instance, unpack and register
UAVDataObject *instobj = dobj->clone(instId); UAVDataObject *instObj = dataObj->clone(instId);
if (!objMngr->registerObject(instobj)) { if (!objMngr->registerObject(instObj)) {
qWarning() << "UAVTalk - Failed to register object " << instObj->toStringBrief();
return NULL; return NULL;
} }
instobj->unpack(data); instObj->unpack(data);
return instobj; return instObj;
} else { } else {
// Unpack data into object instance // Unpack data into object instance
obj->unpack(data); 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. * 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); Transaction *trans = findTransaction(objId, instId);
if (trans && trans->respType == type) { 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); closeTransaction(trans);
delete trans; emit transactionCompleted(obj, false);
emit transactionCompleted(obj, true);
} }
} }
@ -700,10 +707,12 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje
if (allInstances) { if (allInstances) {
// Get number of instances // Get number of instances
quint32 numInst = objMngr->getNumInstances(objId); 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) { for (quint32 n = 0; n < numInst; ++n) {
UAVObject *o = objMngr->getObject(objId, n); quint32 i = numInst - n - 1;
transmitSingleObject(type, objId, n, o); UAVObject *o = objMngr->getObject(objId, i);
transmitSingleObject(type, objId, i, o);
} }
return true; return true;
} else { } else {
@ -736,7 +745,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U
qint32 dataOffset; qint32 dataOffset;
#ifdef VERBOSE_UAVTALK #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 #endif
// IMPORTANT : obj can be null (when type is NACK for example) // 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 // Lookup the transaction in the transaction map
QMap<quint32, Transaction *> *objTransactions = transMap.value(objId); QMap<quint32, Transaction *> *objTransactions = transMap.value(objId);
if (objTransactions != NULL) { 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; 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); QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
if (objTransactions == NULL) { if (objTransactions == NULL) {
objTransactions = new QMap<quint32, Transaction *>(); objTransactions = new QMap<quint32, Transaction *>();
@ -855,6 +874,7 @@ void UAVTalk::closeTransaction(Transaction *trans)
objTransactions->remove(trans->respInstId); objTransactions->remove(trans->respInstId);
// Keep the map even if it is empty // Keep the map even if it is empty
// There are at most 100 different object IDs... // 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 Q_OBJECT
public: public:
static const quint16 ALL_INSTANCES = 0xFFFF;
typedef struct { typedef struct {
quint32 txBytes; quint32 txBytes;
quint32 rxBytes; quint32 rxBytes;
@ -93,8 +95,6 @@ private:
static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH); 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 quint8 INSTANCE_LENGTH = 2;
static const int TX_BUFFER_SIZE = 2 * 1024; 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); bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length);
UAVObject *updateObject(quint32 objId, quint16 instId, quint8 *data); UAVObject *updateObject(quint32 objId, quint16 instId, quint8 *data);
void updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj); 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 transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
bool transmitSingleObject(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);
quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length); quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length);
Transaction *findTransaction(quint32 objId, quint16 instId); Transaction *findTransaction(quint32 objId, quint16 instId);
void startTransaction(Transaction *trans); void openTransaction(quint8 type, quint32 objId, quint16 instId);
void closeTransaction(Transaction *trans); void closeTransaction(Transaction *trans);
void closeAllTransactions(); void closeAllTransactions();
}; };