diff --git a/ground/src/plugins/serialconnection/serialplugin.cpp b/ground/src/plugins/serialconnection/serialplugin.cpp index f7d76589a..85f725471 100644 --- a/ground/src/plugins/serialconnection/serialplugin.cpp +++ b/ground/src/plugins/serialconnection/serialplugin.cpp @@ -77,7 +77,14 @@ QIODevice *SerialConnection::openDevice(const QString &deviceName) if(port.friendName == deviceName) { //we need to handle port settings here... - return new QextSerialPort(port.portName); + PortSettings set; + set.BaudRate = BAUD57600; + set.DataBits = DATA_8; + set.Parity = PAR_NONE; + set.StopBits = STOP_1; + set.FlowControl = FLOW_OFF; + set.Timeout_Millisec = 500; + return new QextSerialPort(port.portName, set); } } return NULL; diff --git a/ground/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp b/ground/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp index 8ecf055a5..252d3035d 100644 --- a/ground/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp +++ b/ground/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp @@ -113,7 +113,7 @@ void UAVObjectTreeModel::addMetaObject(UAVMetaObject *obj, TreeItem *parent) void UAVObjectTreeModel::addInstance(UAVObject *obj, TreeItem *parent) { - connect(obj, SIGNAL(objectUpdatedAuto(UAVObject*)), this, SLOT(highlightUpdatedObject(UAVObject*))); + connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(highlightUpdatedObject(UAVObject*))); TreeItem *item; if (obj->isSingleInstance()) { item = parent; diff --git a/ground/src/plugins/uavtalk/telemetry.cpp b/ground/src/plugins/uavtalk/telemetry.cpp index 33e34005c..c8e6aa8cf 100644 --- a/ground/src/plugins/uavtalk/telemetry.cpp +++ b/ground/src/plugins/uavtalk/telemetry.cpp @@ -44,24 +44,21 @@ Telemetry::Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr) registerObject(objs[objidx][0]); // we only need to register one instance per object type } // Listen to new object creations - connect(objMngr, SIGNAL(newObject(UAVObject*)), this, SLOT(newObject(UAVObject*)), Qt::QueuedConnection); - connect(objMngr, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*)), Qt::QueuedConnection); - // Setup and start the timer + 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*)), this, SLOT(transactionCompleted(UAVObject*))); + // Setup transaction timer + transPending = false; + transTimer = new QTimer(this); + transTimer->stop(); + connect(transTimer, SIGNAL(timeout()), this, SLOT(transactionTimeout())); + // Setup and start the periodic timer timeToNextUpdateMs = 0; - timer = new QTimer(this); - connect(timer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates())); - timer->start(1000); - // Start thread - start(); -} + updateTimer = new QTimer(this); + connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates())); + updateTimer->start(1000); -/** - * Event loop - */ -void Telemetry::run() -{ - // Start main event loop - exec(); } /** @@ -128,19 +125,19 @@ void Telemetry::connectToObjectInstances(UAVObject* obj, quint32 eventMask) // Connect only the selected events if ( (eventMask&EV_UNPACKED) != 0) { - connect(objs[n], SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(objectUnpacked(UAVObject*)), Qt::QueuedConnection); + 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*)), Qt::QueuedConnection); + 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*)), Qt::QueuedConnection); + connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject*)), this, SLOT(objectUpdatedManual(UAVObject*))); } if ( (eventMask&EV_UPDATE_REQ) != 0) { - connect(objs[n], SIGNAL(updateRequested(UAVObject*)), this, SLOT(updateRequested(UAVObject*)), Qt::QueuedConnection); + connect(objs[n], SIGNAL(updateRequested(UAVObject*)), this, SLOT(updateRequested(UAVObject*))); } } } @@ -155,7 +152,7 @@ void Telemetry::updateObject(UAVObject* obj) // Setup object depending on update mode qint32 eventMask; - if( metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_PERIODIC ) + if ( metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_PERIODIC ) { // Set update period setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod); @@ -167,7 +164,7 @@ void Telemetry::updateObject(UAVObject* obj) } connectToObjectInstances(obj, eventMask); } - else if(metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_ONCHANGE) + else if ( metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_ONCHANGE ) { // Set update period setUpdatePeriod(obj, 0); @@ -179,7 +176,7 @@ void Telemetry::updateObject(UAVObject* obj) } connectToObjectInstances(obj, eventMask); } - else if(metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_MANUAL) + else if ( metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_MANUAL ) { // Set update period setUpdatePeriod(obj, 0); @@ -191,7 +188,7 @@ void Telemetry::updateObject(UAVObject* obj) } connectToObjectInstances(obj, eventMask); } - else if(metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_NEVER) + else if ( metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_NEVER ) { // Set update period setUpdatePeriod(obj, 0); @@ -200,38 +197,121 @@ void Telemetry::updateObject(UAVObject* obj) } } +/** + * Called when a transaction is successfully completed (uavtalk event) + */ +void Telemetry::transactionCompleted(UAVObject* obj) +{ + // Check if there is a pending transaction and the objects match + if ( transPending && transInfo.obj->getObjID() == obj->getObjID() ) + { + // Complete transaction + transTimer->stop(); + transPending = false; + // Process new object updates from queue + processObjectQueue(); + } +} + +/** + * Called when a transaction is not completed within the timeout period (timer event) + */ +void Telemetry::transactionTimeout() +{ + transTimer->stop(); + // Proceed only if there is a pending transaction + if ( transPending ) + { + // Check if more retries are pending + if (transInfo.retriesRemaining > 0) + { + --transInfo.retriesRemaining; + processObjectTransaction(); + } + else + { + // Terminate transaction + utalk->cancelTransaction(); + transPending = false; + // Process new object updates from queue + processObjectQueue(); + } + } +} + +/** + * Start an object transaction with UAVTalk, all information is stored in transInfo + */ +void Telemetry::processObjectTransaction() +{ + if (transPending) + { + // Initiate transaction + if (transInfo.objRequest) + { + utalk->sendObjectRequest(transInfo.obj, transInfo.allInstances); + } + else + { + UAVObject::Metadata metadata = transInfo.obj->getMetadata(); + utalk->sendObject(transInfo.obj, metadata.gcsTelemetryAcked, transInfo.allInstances); + } + // Start timer + transTimer->start(REQ_TIMEOUT_MS); + } +} + /** * Process the event received from an object */ void Telemetry::processObjectUpdates(UAVObject* obj, EventMask event, bool allInstances) { - qint32 retries = 0; - bool success = false; - // Get object metadata - UAVObject::Metadata metadata = obj->getMetadata(); - // Act on event - if(event == EV_UPDATED || event == EV_UPDATED_MANUAL) + // Push event into queue + ObjectQueueInfo objInfo; + objInfo.obj = obj; + objInfo.event = event; + objInfo.allInstances = allInstances; + objQueue.enqueue(objInfo); + + // If there is no transaction in progress then process event + if (!transPending) { - // Send update to autopilot (with retries) - retries = 0; - while(retries < MAX_RETRIES && !success) - { - success = utalk->sendObject(obj, metadata.gcsTelemetryAcked, REQ_TIMEOUT_MS, allInstances); // call blocks until ack is received or timeout - ++retries; - } + processObjectQueue(); } - else if(event == EV_UPDATE_REQ) +} + +/** + * Process events from the object queue + */ +void Telemetry::processObjectQueue() +{ + // If the queue is empty there is nothing to do + if (objQueue.isEmpty()) { - // Request object update from autopilot (with retries) - retries = 0; - while(retries < MAX_RETRIES && !success) - { - success = utalk->sendObjectRequest(obj, REQ_TIMEOUT_MS, allInstances); // call blocks until update is received or timeout - ++retries; - } + return; } + + // Get updated object from the queue + ObjectQueueInfo objInfo = objQueue.dequeue(); + + // Setup transaction + transInfo.obj = objInfo.obj; + transInfo.allInstances = objInfo.allInstances; + transInfo.retriesRemaining = MAX_RETRIES; + if ( objInfo.event == EV_UPDATED || objInfo.event == EV_UPDATED_MANUAL ) + { + transInfo.objRequest = false; + } + else if ( objInfo.event == EV_UPDATE_REQ ) + { + transInfo.objRequest = true; + } + // Start transaction + transPending = true; + processObjectTransaction(); + // If this is a metaobject then make necessary telemetry updates - UAVMetaObject* metaobj = dynamic_cast(obj); + UAVMetaObject* metaobj = dynamic_cast(objInfo.obj); if ( metaobj != NULL ) { updateObject( metaobj->getParentObject() ); @@ -240,13 +320,14 @@ void Telemetry::processObjectUpdates(UAVObject* obj, EventMask event, bool allIn /** * Check is any objects are pending for periodic updates + * TODO: Clean-up */ void Telemetry::processPeriodicUpdates() { QMutexLocker locker(mutex); // Stop timer - timer->stop(); + updateTimer->stop(); // Iterate through each object and update its timer, if zero then transmit object. // Also calculate smallest delay to next update (will be used for setting timeToNextUpdateMs) @@ -291,7 +372,7 @@ void Telemetry::processPeriodicUpdates() timeToNextUpdateMs = minDelay; // Restart timer - timer->start(timeToNextUpdateMs); + updateTimer->start(timeToNextUpdateMs); } void Telemetry::objectUpdatedAuto(UAVObject* obj) diff --git a/ground/src/plugins/uavtalk/telemetry.h b/ground/src/plugins/uavtalk/telemetry.h index 471c3125e..28b8bd520 100644 --- a/ground/src/plugins/uavtalk/telemetry.h +++ b/ground/src/plugins/uavtalk/telemetry.h @@ -33,16 +33,15 @@ #include "uavobjects/uavobjectmanager.h" #include #include -#include #include +#include -class Telemetry: public QThread +class Telemetry: public QObject { Q_OBJECT public: Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr); - void run(); signals: @@ -54,10 +53,12 @@ private slots: void newObject(UAVObject* obj); void newInstance(UAVObject* obj); void processPeriodicUpdates(); + void transactionCompleted(UAVObject* obj); + void transactionTimeout(); private: // Constants - static const int REQ_TIMEOUT_MS = 500; + static const int REQ_TIMEOUT_MS = 250; static const int MAX_RETRIES = 3; static const int MAX_UPDATE_PERIOD_MS = 1000; static const int MIN_UPDATE_PERIOD_MS = 1; @@ -79,12 +80,29 @@ private: qint32 timeToNextUpdateMs; /** Time delay to the next update */ } ObjectTimeInfo; + typedef struct { + UAVObject* obj; + EventMask event; + bool allInstances; + } ObjectQueueInfo; + + typedef struct { + UAVObject* obj; + bool allInstances; + bool objRequest; + qint32 retriesRemaining; + } ObjectTransactionInfo; + // Variables UAVObjectManager* objMngr; UAVTalk* utalk; QList objList; + QQueue objQueue; + ObjectTransactionInfo transInfo; + bool transPending; QMutex* mutex; - QTimer* timer; + QTimer* updateTimer; + QTimer* transTimer; qint32 timeToNextUpdateMs; // Methods @@ -94,6 +112,8 @@ private: void connectToObjectInstances(UAVObject* obj, quint32 eventMask); void updateObject(UAVObject* obj); void processObjectUpdates(UAVObject* obj, EventMask event, bool allInstances); + void processObjectTransaction(); + void processObjectQueue(); }; diff --git a/ground/src/plugins/uavtalk/uavtalk.cpp b/ground/src/plugins/uavtalk/uavtalk.cpp index 8e24203d1..1a8445e80 100644 --- a/ground/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/src/plugins/uavtalk/uavtalk.cpp @@ -37,7 +37,6 @@ UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr) this->objMngr = objMngr; rxState = STATE_SYNC; mutex = new QMutex(QMutex::Recursive); - respSema = new QSemaphore(0); respObj = NULL; connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); } @@ -60,35 +59,44 @@ void UAVTalk::processInputStream() * 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 (true), Failure (false) */ -bool UAVTalk::sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances) +bool UAVTalk::sendObjectRequest(UAVObject* obj, bool allInstances) { - return objectTransaction(obj, TYPE_OBJ_REQ, timeout, allInstances); + QMutexLocker locker(mutex); + return objectTransaction(obj, TYPE_OBJ_REQ, 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 Success (true), Failure (false) */ -bool UAVTalk::sendObject(UAVObject* obj, bool acked, qint32 timeoutMs, bool allInstances) +bool UAVTalk::sendObject(UAVObject* obj, bool acked, bool allInstances) { + QMutexLocker locker(mutex); if (acked) { - return objectTransaction(obj, TYPE_OBJ_ACK, timeoutMs, allInstances); + return objectTransaction(obj, TYPE_OBJ_ACK, allInstances); } else { - return objectTransaction(obj, TYPE_OBJ, timeoutMs, allInstances); + return objectTransaction(obj, TYPE_OBJ, allInstances); } } +/** + * Cancel a pending transaction + */ +void UAVTalk::cancelTransaction() +{ + QMutexLocker locker(mutex); + respObj = NULL; +} + /** * Execute the requested transaction on an object. * \param[in] obj Object @@ -97,42 +105,30 @@ bool UAVTalk::sendObject(UAVObject* obj, bool acked, qint32 timeoutMs, bool allI * 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 UAVTalk::objectTransaction(UAVObject* obj, quint8 type, 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) { - respSema->tryAcquire(); // non blocking call to make sure the value is zero (binary sema) 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 - respReceived = respSema->tryAcquire(1, timeoutMs); // lock on object until a response is received (or timeout) - return respReceived; + respAllInstances = allInstances; + return true; } else { - mutex->unlock(); return false; } } else if (type == TYPE_OBJ) { - bool success = transmitObject(obj, TYPE_OBJ, allInstances); - mutex->unlock(); - return success; + return transmitObject(obj, TYPE_OBJ, allInstances); } else { - mutex->unlock(); return false; } } @@ -410,7 +406,6 @@ void UAVTalk::updateAck(UAVObject* obj) { if (respObj != NULL && respObj->getObjID() == obj->getObjID() && (respObj->getInstID() == obj->getInstID() || respAllInstances)) { - respSema->release(); respObj = NULL; emit transactionCompleted(obj); } diff --git a/ground/src/plugins/uavtalk/uavtalk.h b/ground/src/plugins/uavtalk/uavtalk.h index 5fe8b77f6..9a573c086 100644 --- a/ground/src/plugins/uavtalk/uavtalk.h +++ b/ground/src/plugins/uavtalk/uavtalk.h @@ -41,8 +41,9 @@ class UAVTalk: public QObject public: UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr); - bool sendObject(UAVObject* obj, bool acked, qint32 timeoutMs, bool allInstances); - bool sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances); + bool sendObject(UAVObject* obj, bool acked, bool allInstances); + bool sendObjectRequest(UAVObject* obj, bool allInstances); + void cancelTransaction(); signals: void transactionCompleted(UAVObject* obj); @@ -70,7 +71,6 @@ private: QIODevice* io; UAVObjectManager* objMngr; QMutex* mutex; - QSemaphore* respSema; UAVObject* respObj; bool respAllInstances; quint8 rxBuffer[MAX_PACKET_LENGTH]; @@ -86,7 +86,7 @@ private: RxStateType rxState; // Methods - bool objectTransaction(UAVObject* obj, quint8 type, qint32 timeoutMs, bool allInstances); + bool objectTransaction(UAVObject* obj, quint8 type, 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); diff --git a/ground/src/plugins/uavtalk/uavtalkplugin.cpp b/ground/src/plugins/uavtalk/uavtalkplugin.cpp index 2c649215f..45e521cdd 100644 --- a/ground/src/plugins/uavtalk/uavtalkplugin.cpp +++ b/ground/src/plugins/uavtalk/uavtalkplugin.cpp @@ -52,8 +52,6 @@ void UAVTalkPlugin::extensionsInitialized() this, SLOT(onDeviceConnect(QIODevice *))); QObject::connect(cm, SIGNAL(deviceDisconnected()), this, SLOT(onDeviceDisconnect())); - - } bool UAVTalkPlugin::initialize(const QStringList & arguments, QString * errorString) @@ -74,6 +72,7 @@ void UAVTalkPlugin::onDeviceConnect(QIODevice *dev) utalk = new UAVTalk(dev, objMngr); telemetry = new Telemetry(utalk, objMngr); } + void UAVTalkPlugin::onDeviceDisconnect() { delete telemetry;