1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00

UAVTalk GCS plug-in: implemented Telemetry class and small changes in UAVTalk (not yet tested, waiting USB integration)

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@317 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2010-03-15 01:52:42 +00:00 committed by vassilis
parent bc786ba52b
commit 4e5ce0c772
6 changed files with 1034 additions and 590 deletions

View File

@ -25,3 +25,311 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "telemetry.h"
#include <QTime>
/**
* Constructor
*/
Telemetry::Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr)
{
this->utalk = utalk;
this->objMngr = objMngr;
mutex = new QMutex(QMutex::Recursive);
// Process 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
}
// 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
timeToNextUpdateMs = 0;
timer = new QTimer(this);
connect(timer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates()));
timer->start(1000);
// Start thread
start();
}
/**
* Event loop
*/
void Telemetry::run()
{
// Start main event loop
exec();
}
/**
* Register a new object for periodic updates (if enabled)
*/
void Telemetry::registerObject(UAVObject* obj)
{
// Setup object for periodic updates
addObject(obj);
// Setup object for telemetry updates
updateObject(obj);
}
/**
* Add an object in the list used for periodic updates
*/
void Telemetry::addObject(UAVObject* obj)
{
// Check if object type is already in the list
for (int n = 0; n < objList.length(); ++n)
{
if ( objList[n].obj->getObjID() == obj->getObjID() )
{
// Object type (not instance!) is already in the list, do nothing
return;
}
}
// If this point is reached, then the object type is new, let's add it
ObjectTimeInfo timeInfo;
timeInfo.obj = obj;
timeInfo.timeToNextUpdateMs = 0;
timeInfo.updatePeriodMs = 0;
objList.append(timeInfo);
}
/**
* Update the object's timers
*/
void Telemetry::setUpdatePeriod(UAVObject* obj, qint32 periodMs)
{
// Find object type (not instance!) and update its period
for (int n = 0; n < objList.length(); ++n)
{
if ( objList[n].obj->getObjID() == obj->getObjID() )
{
objList[n].updatePeriodMs = periodMs;
objList[n].timeToNextUpdateMs = 0;
}
}
}
/**
* Connect to all instances of an object depending on the event mask specified
*/
void Telemetry::connectToObjectInstances(UAVObject* obj, quint32 eventMask)
{
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*)), Qt::QueuedConnection);
}
if ( (eventMask&EV_UPDATED) != 0)
{
connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject*)), this, SLOT(objectUpdatedAuto(UAVObject*)), Qt::QueuedConnection);
}
if ( (eventMask&EV_UPDATED_MANUAL) != 0)
{
connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject*)), this, SLOT(objectUpdatedManual(UAVObject*)), Qt::QueuedConnection);
}
if ( (eventMask&EV_UPDATE_REQ) != 0)
{
connect(objs[n], SIGNAL(objectUpdateRequested(UAVObject*)), this, SLOT(objectUpdateRequested(UAVObject*)), Qt::QueuedConnection);
}
}
}
/**
* Update an object based on its metadata properties
*/
void Telemetry::updateObject(UAVObject* obj)
{
// Get metadata
UAVObject::Metadata metadata = obj->getMetadata();
// Setup object depending on update mode
qint32 eventMask;
if( metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_PERIODIC )
{
// Set update period
setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
// Connect signals for all instances
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)
}
connectToObjectInstances(obj, eventMask);
}
else if(metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_ONCHANGE)
{
// Set update period
setUpdatePeriod(obj, 0);
// Connect signals for all instances
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)
}
connectToObjectInstances(obj, eventMask);
}
else if(metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_MANUAL)
{
// Set update period
setUpdatePeriod(obj, 0);
// Connect signals for all instances
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)
}
connectToObjectInstances(obj, eventMask);
}
else if(metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_NEVER)
{
// Set update period
setUpdatePeriod(obj, 0);
// Disconnect from object
connectToObjectInstances(obj, 0);
}
}
/**
* 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)
{
// Send update to autopilot (with retries)
retries = 0;
while(retries < MAX_RETRIES && !success)
{
success = utalk->sendObject(obj, metadata.ackRequired, REQ_TIMEOUT_MS, allInstances); // call blocks until ack is received or timeout
++retries;
}
}
else if(event == EV_UPDATE_REQ)
{
// 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;
}
}
// If this is a metaobject then make necessary telemetry updates
UAVMetaObject* metaobj = dynamic_cast<UAVMetaObject*>(obj);
if ( metaobj != NULL )
{
updateObject( metaobj->getParentObject() );
}
}
/**
* Check is any objects are pending for periodic updates
*/
void Telemetry::processPeriodicUpdates()
{
QMutexLocker locker(mutex);
// Stop timer
timer->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)
qint32 minDelay = MAX_UPDATE_PERIOD_MS;
ObjectTimeInfo objinfo;
qint32 elapsedMs = 0;
QTime time;
for (int n = 0; n < objList.length(); ++n)
{
objinfo = objList[n];
// If object is configured for periodic updates
if (objinfo.updatePeriodMs > 0)
{
objinfo.timeToNextUpdateMs -= timeToNextUpdateMs;
// Check if time for the next update
if (objinfo.timeToNextUpdateMs <= 0)
{
// Reset timer
objinfo.timeToNextUpdateMs = objinfo.updatePeriodMs;
// Send object
time.start();
processObjectUpdates(objinfo.obj, EV_UPDATED_MANUAL, true);
elapsedMs = time.elapsed();
// Update timeToNextUpdateMs with the elapsed delay of sending the object;
timeToNextUpdateMs += elapsedMs;
}
// Update minimum delay
if (objinfo.timeToNextUpdateMs < minDelay)
{
minDelay = objinfo.timeToNextUpdateMs;
}
}
}
// Check if delay for the next update is too short
if (minDelay < MIN_UPDATE_PERIOD_MS)
{
minDelay = MIN_UPDATE_PERIOD_MS;
}
// Done
timeToNextUpdateMs = minDelay;
// Restart timer
timer->start(timeToNextUpdateMs);
}
void Telemetry::objectUpdatedAuto(UAVObject* obj)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UPDATED, false);
}
void Telemetry::objectUpdatedManual(UAVObject* obj)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UPDATED_MANUAL, false);
}
void Telemetry::objectUnpacked(UAVObject* obj)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UNPACKED, false);
}
void Telemetry::updateRequested(UAVObject* obj)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UPDATE_REQ, false);
}
void Telemetry::newObject(UAVObject* obj)
{
QMutexLocker locker(mutex);
registerObject(obj);
}
void Telemetry::newInstance(UAVObject* obj)
{
QMutexLocker locker(mutex);
registerObject(obj);
}

View File

@ -25,3 +25,77 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef TELEMETRY_H
#define TELEMETRY_H
#include "uavtalk.h"
#include "uavobjects/uavobjectmanager.h"
#include <QMutex>
#include <QMutexLocker>
#include <QThread>
#include <QTimer>
class Telemetry: public QThread
{
Q_OBJECT
public:
Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr);
void run();
signals:
private slots:
void objectUpdatedAuto(UAVObject* obj);
void objectUpdatedManual(UAVObject* obj);
void objectUnpacked(UAVObject* obj);
void updateRequested(UAVObject* obj);
void newObject(UAVObject* obj);
void newInstance(UAVObject* obj);
void processPeriodicUpdates();
private:
// Constants
static const int REQ_TIMEOUT_MS = 500;
static const int MAX_RETRIES = 3;
static const int MAX_UPDATE_PERIOD_MS = 1000;
static const int MIN_UPDATE_PERIOD_MS = 1;
// Types
/**
* Events generated by objects
*/
typedef enum {
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_UPDATE_REQ = 0x08 /** Request to update object data */
} EventMask;
typedef struct {
UAVObject* obj;
qint32 updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */
qint32 timeToNextUpdateMs; /** Time delay to the next update */
} ObjectTimeInfo;
// Variables
UAVObjectManager* objMngr;
UAVTalk* utalk;
QList<ObjectTimeInfo> objList;
QMutex* mutex;
QTimer* timer;
qint32 timeToNextUpdateMs;
// Methods
void registerObject(UAVObject* obj);
void addObject(UAVObject* obj);
void setUpdatePeriod(UAVObject* obj, qint32 periodMs);
void connectToObjectInstances(UAVObject* obj, quint32 eventMask);
void updateObject(UAVObject* obj);
void processObjectUpdates(UAVObject* obj, EventMask event, bool allInstances);
};
#endif // TELEMETRY_H

View File

@ -28,6 +28,9 @@
#include "uavtalk.h"
#include <QtEndian>
/**
* Constructor
*/
UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr)
{
io = iodev;
@ -39,6 +42,9 @@ UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr)
connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream()));
}
/**
* Called each time there are data in the input buffer
*/
void UAVTalk::processInputStream()
{
quint8 tmp;
@ -56,9 +62,9 @@ void UAVTalk::processInputStream()
* \param[in] obj Object to update
* \param[in] timeout Time to wait for the response, when zero it will return immediately
* \param[in] allInstances If set true then all instances will be updated
* \return Success (0), Failure (-1)
* \return Success (true), Failure (false)
*/
qint32 UAVTalk::sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances)
bool UAVTalk::sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances)
{
return objectTransaction(obj, TYPE_OBJ_REQ, timeout, allInstances);
}
@ -69,18 +75,17 @@ qint32 UAVTalk::sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstan
* \param[in] acked Selects if an ack is required
* \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
* \param[in] allInstances If set true then all instances will be updated
* \return 0 Success
* \return -1 Failure
* \return Success (true), Failure (false)
*/
qint32 UAVTalk::sendObject(UAVObject* obj, bool acked, qint32 timeoutMs)
bool UAVTalk::sendObject(UAVObject* obj, bool acked, qint32 timeoutMs, bool allInstances)
{
if (acked)
{
return objectTransaction(obj, TYPE_OBJ_ACK, timeoutMs, false);
return objectTransaction(obj, TYPE_OBJ_ACK, timeoutMs, allInstances);
}
else
{
return objectTransaction(obj, TYPE_OBJ, timeoutMs, false);
return objectTransaction(obj, TYPE_OBJ, timeoutMs, allInstances);
}
}
@ -278,7 +283,14 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8*
// Get object and update its data
obj = updateObject(objId, instId, data);
// Check if an ack is pending
updateAck(obj);
if ( obj != NULL )
{
updateAck(obj);
}
else
{
error = true;
}
}
else
{
@ -292,7 +304,14 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8*
// Get object and update its data
obj = updateObject(objId, instId, data);
// Transmit ACK
transmitObject(obj, TYPE_ACK, false);
if ( obj != NULL )
{
transmitObject(obj, TYPE_ACK, false);
}
else
{
error = true;
}
}
else
{
@ -343,7 +362,11 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8*
return !error;
}
/**
* Update the data of an object from a byte array (unpack).
* If the object instance could not be found in the list, then a
* new one is created.
*/
UAVObject* UAVTalk::updateObject(quint32 objId, quint16 instId, quint8* data)
{
// Get object
@ -351,13 +374,38 @@ UAVObject* UAVTalk::updateObject(quint32 objId, quint16 instId, quint8* data)
// If the instance does not exist create it
if (obj == NULL)
{
obj = objMngr->newObjectInstance(objId, instId);
// Get the object type
UAVObject* tobj = objMngr->getObject(objId);
if (tobj == NULL)
{
return NULL;
}
// Make sure this is a data object
UAVDataObject* dobj = dynamic_cast<UAVDataObject*>(tobj);
if (dobj == NULL)
{
return NULL;
}
// Create a new instance, unpack and register
UAVDataObject* instobj = dobj->clone(instId);
if ( !objMngr->registerObject(instobj) )
{
return NULL;
}
instobj->unpack(data);
return instobj;
}
else
{
// Unpack data into object instance
obj->unpack(data);
return obj;
}
// Unpack data into object instance
obj->unpack(data);
return obj;
}
/**
* Check if a transaction is pending and if yes complete it.
*/
void UAVTalk::updateAck(UAVObject* obj)
{
if (respObj != NULL && respObj->getObjID() == obj->getObjID() && (respObj->getInstID() == obj->getInstID() || respAllInstances))
@ -405,13 +453,13 @@ bool UAVTalk::transmitObject(UAVObject* obj, quint8 type, bool allInstances)
}
else if (type == TYPE_OBJ_REQ)
{
return transmitSingleObject(obj, type, allInstances);
return transmitSingleObject(obj, TYPE_OBJ_REQ, allInstances);
}
else if (type == TYPE_ACK)
{
if (!allInstances)
{
return transmitSingleObject(obj, type, false);
return transmitSingleObject(obj, TYPE_ACK, false);
}
else
{

View File

@ -41,8 +41,8 @@ class UAVTalk: public QObject
public:
UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr);
qint32 sendObject(UAVObject* obj, bool acked, qint32 timeoutMs);
qint32 sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances);
bool sendObject(UAVObject* obj, bool acked, qint32 timeoutMs, bool allInstances);
bool sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances);
signals:
void transactionCompleted(UAVObject* obj);

View File

@ -25,7 +25,6 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "uavtalkplugin.h"
UAVTalkPlugin::UAVTalkPlugin()
@ -40,14 +39,24 @@ UAVTalkPlugin::~UAVTalkPlugin()
void UAVTalkPlugin::extensionsInitialized()
{
// Get UAVObjectManager instance
ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance();
objMngr = pm->getObject<UAVObjectManager>();
// TODO: Initialize serial port and USB libraries, get QIODevice from each
// TODO: Initialize UAVTalk object
//utalk = new UAVTalk(io, objMngr);
// TODO: Initialize telemetry object
//telemetry = new Telemetry(utalk, objMngr);
}
bool UAVTalkPlugin::initialize(const QStringList & arguments, QString * errorString)
{
// Done
Q_UNUSED(arguments);
Q_UNUSED(errorString);
return true;
}

View File

@ -28,15 +28,15 @@
#ifndef UAVTALKPLUGIN_H
#define UAVTALKPLUGIN_H
#include <extensionsystem/iplugin.h>
#include <extensionsystem/pluginmanager.h>
#include <QtPlugin>
#include "telemetry.h"
#include "uavtalk.h"
#include "uavobjects/uavobjectmanager.h"
class UAVTalkPlugin:
public ExtensionSystem::IPlugin
class UAVTalkPlugin: public ExtensionSystem::IPlugin
{
Q_OBJECT
public:
UAVTalkPlugin();
~UAVTalkPlugin();
@ -44,6 +44,11 @@ public:
void extensionsInitialized();
bool initialize(const QStringList & arguments, QString * errorString);
void shutdown();
private:
UAVObjectManager* objMngr;
UAVTalk* utalk;
Telemetry* telemetry;
};
#endif // UAVTALKPLUGIN_H