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

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

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,3 +25,311 @@
* with this program; if not, write to the Free Software Foundation, Inc., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 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);
}

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

@ -25,3 +25,77 @@
* with this program; if not, write to the Free Software Foundation, Inc., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 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

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

@ -25,513 +25,561 @@
* with this program; if not, write to the Free Software Foundation, Inc., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#include "uavtalk.h" #include "uavtalk.h"
#include <QtEndian> #include <QtEndian>
UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr) /**
{ * Constructor
io = iodev; */
this->objMngr = objMngr; UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr)
rxState = STATE_SYNC; {
mutex = new QMutex(QMutex::Recursive); io = iodev;
respSema = new QSemaphore(0); this->objMngr = objMngr;
respObj = NULL; rxState = STATE_SYNC;
connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream())); mutex = new QMutex(QMutex::Recursive);
} respSema = new QSemaphore(0);
respObj = NULL;
void UAVTalk::processInputStream() connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream()));
{ }
quint8 tmp;
/**
while (io->bytesAvailable() > 0) * Called each time there are data in the input buffer
{ */
io->read((char*)&tmp, 1); void UAVTalk::processInputStream()
processInputByte(tmp); {
} quint8 tmp;
}
while (io->bytesAvailable() > 0)
/** {
* Request an update for the specified object, on success the object data would have been io->read((char*)&tmp, 1);
* updated by the GCS. processInputByte(tmp);
* \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) /**
*/ * Request an update for the specified object, on success the object data would have been
qint32 UAVTalk::sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances) * updated by the GCS.
{ * \param[in] obj Object to update
return objectTransaction(obj, TYPE_OBJ_REQ, timeout, allInstances); * \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)
/** */
* Send the specified object through the telemetry link. bool UAVTalk::sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances)
* \param[in] obj Object to send {
* \param[in] acked Selects if an ack is required return objectTransaction(obj, TYPE_OBJ_REQ, timeout, allInstances);
* \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 * Send the specified object through the telemetry link.
*/ * \param[in] obj Object to send
qint32 UAVTalk::sendObject(UAVObject* obj, bool acked, qint32 timeoutMs) * \param[in] acked Selects if an ack is required
{ * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
if (acked) * \param[in] allInstances If set true then all instances will be updated
{ * \return Success (true), Failure (false)
return objectTransaction(obj, TYPE_OBJ_ACK, timeoutMs, false); */
} bool UAVTalk::sendObject(UAVObject* obj, bool acked, qint32 timeoutMs, bool allInstances)
else {
{ if (acked)
return objectTransaction(obj, TYPE_OBJ, timeoutMs, false); {
} return objectTransaction(obj, TYPE_OBJ_ACK, timeoutMs, allInstances);
} }
else
/** {
* Execute the requested transaction on an object. return objectTransaction(obj, TYPE_OBJ, timeoutMs, allInstances);
* \param[in] obj Object }
* \param[in] type Transaction type }
* TYPE_OBJ: send object,
* TYPE_OBJ_REQ: request object update /**
* TYPE_OBJ_ACK: send object with an ack * Execute the requested transaction on an object.
* \param[in] allInstances If set true then all instances will be updated * \param[in] obj Object
* \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately * \param[in] type Transaction type
* \return Success (true), Failure (false) * TYPE_OBJ: send object,
*/ * TYPE_OBJ_REQ: request object update
bool UAVTalk::objectTransaction(UAVObject* obj, quint8 type, qint32 timeoutMs, bool allInstances) * TYPE_OBJ_ACK: send object with an ack
{ * \param[in] allInstances If set true then all instances will be updated
bool respReceived; * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
* \return Success (true), Failure (false)
// Lock */
mutex->lock(); bool UAVTalk::objectTransaction(UAVObject* obj, quint8 type, qint32 timeoutMs, bool allInstances)
// Send object depending on if a response is needed {
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) bool respReceived;
{
if ( transmitObject(obj, type, allInstances) ) // Lock
{ mutex->lock();
respObj = obj; // Send object depending on if a response is needed
respAllInstances = allInstances; if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ)
mutex->unlock(); // need to release lock since the next call will block until a response is received {
respSema->tryAcquire(); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema) if ( transmitObject(obj, type, allInstances) )
respReceived = respSema->tryAcquire(1, timeoutMs); // lock on object until a response is received (or timeout) {
return respReceived; respObj = obj;
} respAllInstances = allInstances;
else mutex->unlock(); // need to release lock since the next call will block until a response is received
{ respSema->tryAcquire(); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema)
mutex->unlock(); respReceived = respSema->tryAcquire(1, timeoutMs); // lock on object until a response is received (or timeout)
return false; return respReceived;
} }
} else
else if (type == TYPE_OBJ) {
{ mutex->unlock();
bool success = transmitObject(obj, TYPE_OBJ, allInstances); return false;
mutex->unlock(); }
return success; }
} else if (type == TYPE_OBJ)
else {
{ bool success = transmitObject(obj, TYPE_OBJ, allInstances);
mutex->unlock(); mutex->unlock();
return false; return success;
} }
} else
{
/** mutex->unlock();
* Process an byte from the telemetry stream. return false;
* \param[in] rxbyte Received byte }
* \return Success (true), Failure (false) }
*/
bool UAVTalk::processInputByte(quint8 rxbyte) /**
{ * Process an byte from the telemetry stream.
// Receive state machine * \param[in] rxbyte Received byte
switch (rxState) { * \return Success (true), Failure (false)
case STATE_SYNC: */
if ((rxbyte & TYPE_MASK) == TYPE_VER ) bool UAVTalk::processInputByte(quint8 rxbyte)
{ {
rxCS = rxbyte; // Receive state machine
rxType = rxbyte; switch (rxState) {
rxState = STATE_OBJID; case STATE_SYNC:
rxCount = 0; if ((rxbyte & TYPE_MASK) == TYPE_VER )
} {
break; rxCS = rxbyte;
case STATE_OBJID: rxType = rxbyte;
rxTmpBuffer[rxCount++] = rxbyte; rxState = STATE_OBJID;
if (rxCount == 4) rxCount = 0;
{ }
// Search for object, if not found reset state machine break;
rxObjId = (qint32)qFromBigEndian<qint32>(rxTmpBuffer); case STATE_OBJID:
UAVObject* rxObj = objMngr->getObject(rxObjId); rxTmpBuffer[rxCount++] = rxbyte;
if (rxObj == NULL) if (rxCount == 4)
{ {
rxState = STATE_SYNC; // Search for object, if not found reset state machine
} rxObjId = (qint32)qFromBigEndian<qint32>(rxTmpBuffer);
else UAVObject* rxObj = objMngr->getObject(rxObjId);
{ if (rxObj == NULL)
// Update checksum {
rxCS = updateChecksum(rxCS, rxTmpBuffer, 4); rxState = STATE_SYNC;
// Determine data length }
if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK) else
{ {
rxLength = 0; // Update checksum
} rxCS = updateChecksum(rxCS, rxTmpBuffer, 4);
else // Determine data length
{ if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK)
rxLength = rxObj->getNumBytes(); {
} rxLength = 0;
// Check length and determine next state }
if (rxLength >= MAX_PAYLOAD_LENGTH) else
{ {
rxState = STATE_SYNC; rxLength = rxObj->getNumBytes();
} }
else // Check length and determine next state
{ if (rxLength >= MAX_PAYLOAD_LENGTH)
// Check if this is a single instance object (i.e. if the instance ID field is coming next) {
if ( rxObj->isSingleInstance() ) rxState = STATE_SYNC;
{ }
// If there is a payload get it, otherwise receive checksum else
if (rxLength > 0) {
{ // Check if this is a single instance object (i.e. if the instance ID field is coming next)
rxState = STATE_DATA; if ( rxObj->isSingleInstance() )
} {
else // If there is a payload get it, otherwise receive checksum
{ if (rxLength > 0)
rxState = STATE_CS; {
} rxState = STATE_DATA;
rxInstId = 0; }
rxCount = 0; else
} {
else rxState = STATE_CS;
{ }
rxState = STATE_INSTID; rxInstId = 0;
rxCount = 0; rxCount = 0;
} }
} else
} {
} rxState = STATE_INSTID;
break; rxCount = 0;
case STATE_INSTID: }
rxTmpBuffer[rxCount++] = rxbyte; }
if (rxCount == 2) }
{ }
rxInstId = (qint16)qFromBigEndian<qint16>(rxTmpBuffer); break;
rxCS = updateChecksum(rxCS, rxTmpBuffer, 2); case STATE_INSTID:
rxCount = 0; rxTmpBuffer[rxCount++] = rxbyte;
// If there is a payload get it, otherwise receive checksum if (rxCount == 2)
if (rxLength > 0) {
{ rxInstId = (qint16)qFromBigEndian<qint16>(rxTmpBuffer);
rxState = STATE_DATA; rxCS = updateChecksum(rxCS, rxTmpBuffer, 2);
} rxCount = 0;
else // If there is a payload get it, otherwise receive checksum
{ if (rxLength > 0)
rxState = STATE_CS; {
} rxState = STATE_DATA;
} }
break; else
case STATE_DATA: {
rxBuffer[rxCount++] = rxbyte; rxState = STATE_CS;
if (rxCount == rxLength) }
{ }
rxCS = updateChecksum(rxCS, rxBuffer, rxLength); break;
rxState = STATE_CS; case STATE_DATA:
rxCount = 0; rxBuffer[rxCount++] = rxbyte;
} if (rxCount == rxLength)
break; {
case STATE_CS: rxCS = updateChecksum(rxCS, rxBuffer, rxLength);
rxTmpBuffer[rxCount++] = rxbyte; rxState = STATE_CS;
if (rxCount == 2) rxCount = 0;
{ }
rxCSPacket = (qint16)qFromBigEndian<qint16>(rxTmpBuffer); break;
if (rxCS == rxCSPacket) case STATE_CS:
{ rxTmpBuffer[rxCount++] = rxbyte;
mutex->lock(); if (rxCount == 2)
receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength); {
mutex->unlock(); rxCSPacket = (qint16)qFromBigEndian<qint16>(rxTmpBuffer);
} if (rxCS == rxCSPacket)
rxState = STATE_SYNC; {
} mutex->lock();
break; receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength);
default: mutex->unlock();
rxState = STATE_SYNC; }
} rxState = STATE_SYNC;
}
// Done break;
return true; default:
} rxState = STATE_SYNC;
}
/**
* Receive an object. This function process objects received through the telemetry stream. // Done
* \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK) return true;
* \param[in] obj Handle of the received object }
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] data Data buffer /**
* \param[in] length Buffer length * Receive an object. This function process objects received through the telemetry stream.
* \return Success (true), Failure (false) * \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK)
*/ * \param[in] obj Handle of the received object
bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* data, qint32 length) * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
{ * \param[in] data Data buffer
UAVObject* obj = NULL; * \param[in] length Buffer length
bool error = false; * \return Success (true), Failure (false)
bool allInstances = (instId == ALL_INSTANCES? true : false); */
bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* data, qint32 length)
// Process message type {
switch (type) { UAVObject* obj = NULL;
case TYPE_OBJ: bool error = false;
// All instances, not allowed for OBJ messages bool allInstances = (instId == ALL_INSTANCES? true : false);
if (!allInstances)
{ // Process message type
// Get object and update its data switch (type) {
obj = updateObject(objId, instId, data); case TYPE_OBJ:
// Check if an ack is pending // All instances, not allowed for OBJ messages
updateAck(obj); if (!allInstances)
} {
else // Get object and update its data
{ obj = updateObject(objId, instId, data);
error = true; // Check if an ack is pending
} if ( obj != NULL )
break; {
case TYPE_OBJ_ACK: updateAck(obj);
// All instances, not allowed for OBJ_ACK messages }
if (!allInstances) else
{ {
// Get object and update its data error = true;
obj = updateObject(objId, instId, data); }
// Transmit ACK }
transmitObject(obj, TYPE_ACK, false); else
} {
else error = true;
{ }
error = true; break;
} case TYPE_OBJ_ACK:
break; // All instances, not allowed for OBJ_ACK messages
case TYPE_OBJ_REQ: if (!allInstances)
// Get object, if all instances are requested get instance 0 of the object {
if (allInstances) // Get object and update its data
{ obj = updateObject(objId, instId, data);
obj = objMngr->getObject(objId); // Transmit ACK
} if ( obj != NULL )
else {
{ transmitObject(obj, TYPE_ACK, false);
obj = objMngr->getObject(objId, instId); }
} else
// If object was found transmit it {
if (obj != NULL) error = true;
{ }
transmitObject(obj, TYPE_OBJ, allInstances); }
} else
else {
{ error = true;
error = true; }
} break;
break; case TYPE_OBJ_REQ:
case TYPE_ACK: // Get object, if all instances are requested get instance 0 of the object
// All instances, not allowed for ACK messages if (allInstances)
if (!allInstances) {
{ obj = objMngr->getObject(objId);
// Get object }
obj = objMngr->getObject(objId, instId); else
// Check if an ack is pending {
if (obj != NULL) obj = objMngr->getObject(objId, instId);
{ }
updateAck(obj); // If object was found transmit it
} if (obj != NULL)
else {
{ transmitObject(obj, TYPE_OBJ, allInstances);
error = true; }
} else
} {
break; error = true;
default: }
error = true; break;
} case TYPE_ACK:
// Done // All instances, not allowed for ACK messages
return !error; if (!allInstances)
} {
// Get object
obj = objMngr->getObject(objId, instId);
UAVObject* UAVTalk::updateObject(quint32 objId, quint16 instId, quint8* data) // Check if an ack is pending
{ if (obj != NULL)
// Get object {
UAVObject* obj = objMngr->getObject(objId, instId); updateAck(obj);
// If the instance does not exist create it }
if (obj == NULL) else
{ {
obj = objMngr->newObjectInstance(objId, instId); error = true;
} }
// Unpack data into object instance }
obj->unpack(data); break;
return obj; default:
} error = true;
}
void UAVTalk::updateAck(UAVObject* obj) // Done
{ return !error;
if (respObj != NULL && respObj->getObjID() == obj->getObjID() && (respObj->getInstID() == obj->getInstID() || respAllInstances)) }
{
respSema->release(); /**
respObj = NULL; * Update the data of an object from a byte array (unpack).
emit transactionCompleted(obj); * 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)
/** {
* Send an object through the telemetry link. // Get object
* \param[in] obj Object to send UAVObject* obj = objMngr->getObject(objId, instId);
* \param[in] type Transaction type // If the instance does not exist create it
* \param[in] allInstances True is all instances of the object are to be sent if (obj == NULL)
* \return Success (true), Failure (false) {
*/ // Get the object type
bool UAVTalk::transmitObject(UAVObject* obj, quint8 type, bool allInstances) UAVObject* tobj = objMngr->getObject(objId);
{ if (tobj == NULL)
// If all instances are requested on a single instance object it is an error {
if (allInstances && obj->isSingleInstance()) return NULL;
{ }
return false; // Make sure this is a data object
} UAVDataObject* dobj = dynamic_cast<UAVDataObject*>(tobj);
if (dobj == NULL)
// Process message type {
if ( type == TYPE_OBJ || type == TYPE_OBJ_ACK ) return NULL;
{ }
if (allInstances) // Create a new instance, unpack and register
{ UAVDataObject* instobj = dobj->clone(instId);
// Get number of instances if ( !objMngr->registerObject(instobj) )
quint32 numInst = objMngr->getNumInstances(obj->getObjID()); {
// Send all instances return NULL;
for (quint32 instId = 0; instId < numInst; ++instId) }
{ instobj->unpack(data);
UAVObject* inst = objMngr->getObject(obj->getObjID(), instId); return instobj;
transmitSingleObject(inst, type, false); }
} else
return true; {
} // Unpack data into object instance
else obj->unpack(data);
{ return obj;
return transmitSingleObject(obj, type, false); }
} }
}
else if (type == TYPE_OBJ_REQ) /**
{ * Check if a transaction is pending and if yes complete it.
return transmitSingleObject(obj, type, allInstances); */
} void UAVTalk::updateAck(UAVObject* obj)
else if (type == TYPE_ACK) {
{ if (respObj != NULL && respObj->getObjID() == obj->getObjID() && (respObj->getInstID() == obj->getInstID() || respAllInstances))
if (!allInstances) {
{ respSema->release();
return transmitSingleObject(obj, type, false); respObj = NULL;
} emit transactionCompleted(obj);
else }
{ }
return false;
} /**
} * Send an object through the telemetry link.
else * \param[in] obj Object to send
{ * \param[in] type Transaction type
return false; * \param[in] allInstances True is all instances of the object are to be sent
} * \return Success (true), Failure (false)
} */
bool UAVTalk::transmitObject(UAVObject* obj, quint8 type, bool allInstances)
{
/** // If all instances are requested on a single instance object it is an error
* Send an object through the telemetry link. if (allInstances && obj->isSingleInstance())
* \param[in] obj Object handle to send {
* \param[in] type Transaction type return false;
* \return Success (true), Failure (false) }
*/
bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances) // Process message type
{ if ( type == TYPE_OBJ || type == TYPE_OBJ_ACK )
qint32 length; {
qint32 dataOffset; if (allInstances)
quint16 cs = 0; {
quint32 objId; // Get number of instances
quint16 instId; quint32 numInst = objMngr->getNumInstances(obj->getObjID());
quint16 allInstId = ALL_INSTANCES; // Send all instances
for (quint32 instId = 0; instId < numInst; ++instId)
// Setup type and object id fields {
objId = obj->getObjID(); UAVObject* inst = objMngr->getObject(obj->getObjID(), instId);
txBuffer[0] = type; transmitSingleObject(inst, type, false);
qToBigEndian<qint32>(objId, &txBuffer[1]); }
return true;
// Setup instance ID if one is required }
if ( obj->isSingleInstance() ) else
{ {
dataOffset = 5; return transmitSingleObject(obj, type, false);
} }
else }
{ else if (type == TYPE_OBJ_REQ)
// Check if all instances are requested {
if (allInstances) return transmitSingleObject(obj, TYPE_OBJ_REQ, allInstances);
{ }
qToBigEndian<qint16>(allInstId, &txBuffer[5]); else if (type == TYPE_ACK)
} {
else if (!allInstances)
{ {
instId = obj->getInstID(); return transmitSingleObject(obj, TYPE_ACK, false);
qToBigEndian<qint16>(instId, &txBuffer[5]); }
} else
dataOffset = 7; {
} return false;
}
// Determine data length }
if (type == TYPE_OBJ_REQ || type == TYPE_ACK) else
{ {
length = 0; return false;
} }
else }
{
length = obj->getNumBytes();
} /**
* Send an object through the telemetry link.
// Check length * \param[in] obj Object handle to send
if (length >= MAX_PAYLOAD_LENGTH) * \param[in] type Transaction type
{ * \return Success (true), Failure (false)
return false; */
} bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances)
{
// Copy data (if any) qint32 length;
if (length > 0) qint32 dataOffset;
{ quint16 cs = 0;
if ( !obj->pack(&txBuffer[dataOffset]) ) quint32 objId;
{ quint16 instId;
return false; quint16 allInstId = ALL_INSTANCES;
}
} // Setup type and object id fields
objId = obj->getObjID();
// Calculate checksum txBuffer[0] = type;
cs = 0; qToBigEndian<qint32>(objId, &txBuffer[1]);
cs = updateChecksum(cs, txBuffer, dataOffset+length);
qToBigEndian<qint16>(cs, &txBuffer[dataOffset+length]); // Setup instance ID if one is required
if ( obj->isSingleInstance() )
// Send buffer {
io->write((const char*)txBuffer, dataOffset+length+CHECKSUM_LENGTH); dataOffset = 5;
}
// Done else
return true; {
} // Check if all instances are requested
if (allInstances)
/** {
* Update checksum. qToBigEndian<qint16>(allInstId, &txBuffer[5]);
* TODO: Replace with CRC-16 }
* \param[in] data Data buffer to update checksum on else
* \param[in] length Length of buffer {
* \return Updated checksum instId = obj->getInstID();
*/ qToBigEndian<qint16>(instId, &txBuffer[5]);
quint16 UAVTalk::updateChecksum(quint16 cs, quint8* data, qint32 length) }
{ dataOffset = 7;
qint32 n; }
for (n = 0; n < length; ++n)
{ // Determine data length
cs += (quint16)data[n]; if (type == TYPE_OBJ_REQ || type == TYPE_ACK)
} {
return cs; length = 0;
} }
else
{
length = obj->getNumBytes();
}
// Check length
if (length >= MAX_PAYLOAD_LENGTH)
{
return false;
}
// Copy data (if any)
if (length > 0)
{
if ( !obj->pack(&txBuffer[dataOffset]) )
{
return false;
}
}
// Calculate checksum
cs = 0;
cs = updateChecksum(cs, txBuffer, dataOffset+length);
qToBigEndian<qint16>(cs, &txBuffer[dataOffset+length]);
// Send buffer
io->write((const char*)txBuffer, dataOffset+length+CHECKSUM_LENGTH);
// Done
return true;
}
/**
* Update checksum.
* TODO: Replace with CRC-16
* \param[in] data Data buffer to update checksum on
* \param[in] length Length of buffer
* \return Updated checksum
*/
quint16 UAVTalk::updateChecksum(quint16 cs, quint8* data, qint32 length)
{
qint32 n;
for (n = 0; n < length; ++n)
{
cs += (quint16)data[n];
}
return cs;
}

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

@ -25,76 +25,76 @@
* with this program; if not, write to the Free Software Foundation, Inc., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef UAVTALK_H #ifndef UAVTALK_H
#define UAVTALK_H #define UAVTALK_H
#include <QIODevice> #include <QIODevice>
#include <QMutex> #include <QMutex>
#include <QMutexLocker> #include <QMutexLocker>
#include <QSemaphore> #include <QSemaphore>
#include "uavobjects/uavobjectmanager.h" #include "uavobjects/uavobjectmanager.h"
class UAVTalk: public QObject class UAVTalk: public QObject
{ {
Q_OBJECT Q_OBJECT
public: public:
UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr); UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr);
qint32 sendObject(UAVObject* obj, bool acked, qint32 timeoutMs); bool sendObject(UAVObject* obj, bool acked, qint32 timeoutMs, bool allInstances);
qint32 sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances); bool sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances);
signals: signals:
void transactionCompleted(UAVObject* obj); void transactionCompleted(UAVObject* obj);
private slots: private slots:
void processInputStream(void); void processInputStream(void);
private: private:
// Constants // Constants
static const int TYPE_MASK = 0xFC; static const int TYPE_MASK = 0xFC;
static const int TYPE_VER = 0x10; static const int TYPE_VER = 0x10;
static const int TYPE_OBJ = (TYPE_VER | 0x00); static const int TYPE_OBJ = (TYPE_VER | 0x00);
static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01); static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01);
static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02); static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02);
static const int TYPE_ACK = (TYPE_VER | 0x03); static const int TYPE_ACK = (TYPE_VER | 0x03);
static const int HEADER_LENGTH = 7; // type (1), object ID (4), instance ID (2, not used in single objects) static const int HEADER_LENGTH = 7; // type (1), object ID (4), instance ID (2, not used in single objects)
static const int CHECKSUM_LENGTH = 2; static const int CHECKSUM_LENGTH = 2;
static const int MAX_PAYLOAD_LENGTH = 256; static const int MAX_PAYLOAD_LENGTH = 256;
static const int MAX_PACKET_LENGTH = (HEADER_LENGTH+MAX_PAYLOAD_LENGTH+CHECKSUM_LENGTH); static const int MAX_PACKET_LENGTH = (HEADER_LENGTH+MAX_PAYLOAD_LENGTH+CHECKSUM_LENGTH);
static const quint16 ALL_INSTANCES = 0xFFFF; static const quint16 ALL_INSTANCES = 0xFFFF;
typedef enum {STATE_SYNC, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxStateType; typedef enum {STATE_SYNC, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxStateType;
// Variables // Variables
QIODevice* io; QIODevice* io;
UAVObjectManager* objMngr; UAVObjectManager* objMngr;
QMutex* mutex; QMutex* mutex;
QSemaphore* respSema; QSemaphore* respSema;
UAVObject* respObj; UAVObject* respObj;
bool respAllInstances; bool respAllInstances;
quint8 rxBuffer[MAX_PACKET_LENGTH]; quint8 rxBuffer[MAX_PACKET_LENGTH];
quint8 txBuffer[MAX_PACKET_LENGTH]; quint8 txBuffer[MAX_PACKET_LENGTH];
// Variables used by the receive state machine // Variables used by the receive state machine
quint8 rxTmpBuffer[4]; quint8 rxTmpBuffer[4];
quint8 rxType; quint8 rxType;
quint32 rxObjId; quint32 rxObjId;
quint16 rxInstId; quint16 rxInstId;
quint8 rxLength; quint8 rxLength;
quint16 rxCSPacket, rxCS; quint16 rxCSPacket, rxCS;
qint32 rxCount; qint32 rxCount;
RxStateType rxState; RxStateType rxState;
// Methods // Methods
bool objectTransaction(UAVObject* obj, quint8 type, qint32 timeoutMs, bool allInstances); bool objectTransaction(UAVObject* obj, quint8 type, qint32 timeoutMs, bool allInstances);
bool processInputByte(quint8 rxbyte); bool processInputByte(quint8 rxbyte);
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(UAVObject* obj); void updateAck(UAVObject* obj);
bool transmitObject(UAVObject* obj, quint8 type, bool allInstances); bool transmitObject(UAVObject* obj, quint8 type, bool allInstances);
bool transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances); bool transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances);
quint16 updateChecksum(quint16 cs, quint8* data, qint32 length); quint16 updateChecksum(quint16 cs, quint8* data, qint32 length);
}; };
#endif // UAVTALK_H #endif // UAVTALK_H

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

@ -25,7 +25,6 @@
* with this program; if not, write to the Free Software Foundation, Inc., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#include "uavtalkplugin.h" #include "uavtalkplugin.h"
UAVTalkPlugin::UAVTalkPlugin() UAVTalkPlugin::UAVTalkPlugin()
@ -40,14 +39,24 @@ UAVTalkPlugin::~UAVTalkPlugin()
void UAVTalkPlugin::extensionsInitialized() 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) bool UAVTalkPlugin::initialize(const QStringList & arguments, QString * errorString)
{ {
// Done
Q_UNUSED(arguments); Q_UNUSED(arguments);
Q_UNUSED(errorString); Q_UNUSED(errorString);
return true; return true;
} }

15
ground/src/plugins/uavtalk/uavtalkplugin.h
View File

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