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-4 GCS/Telemetry Refactored telemetry to be fully event driven and various bug fixes (working but not yet fully tested)

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@501 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2010-04-15 02:19:13 +00:00 committed by vassilis
parent 5889849060
commit bd853085c8
7 changed files with 188 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ground/src/plugins/serialconnection/serialplugin.cpp
View File

@ -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;

2
ground/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp
View File

@ -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;

177
ground/src/plugins/uavtalk/telemetry.cpp
View File

@ -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<UAVMetaObject*>(obj);
UAVMetaObject* metaobj = dynamic_cast<UAVMetaObject*>(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)

30
ground/src/plugins/uavtalk/telemetry.h
View File

@ -33,16 +33,15 @@
#include "uavobjects/uavobjectmanager.h"
#include <QMutex>
#include <QMutexLocker>
#include <QThread>
#include <QTimer>
#include <QQueue>
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<ObjectTimeInfo> objList;
QQueue<ObjectQueueInfo> 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();
};

45
ground/src/plugins/uavtalk/uavtalk.cpp
View File

@ -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);
}

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

@ -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);

3
ground/src/plugins/uavtalk/uavtalkplugin.cpp
View File

@ -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;