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
vassilis
parent
bc786ba52b
commit
4e5ce0c772
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -25,513 +25,561 @@
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#include "uavtalk.h"
|
||||
#include <QtEndian>
|
||||
|
||||
UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr)
|
||||
{
|
||||
io = iodev;
|
||||
this->objMngr = objMngr;
|
||||
rxState = STATE_SYNC;
|
||||
mutex = new QMutex(QMutex::Recursive);
|
||||
respSema = new QSemaphore(0);
|
||||
respObj = NULL;
|
||||
connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream()));
|
||||
}
|
||||
|
||||
void UAVTalk::processInputStream()
|
||||
{
|
||||
quint8 tmp;
|
||||
|
||||
while (io->bytesAvailable() > 0)
|
||||
{
|
||||
io->read((char*)&tmp, 1);
|
||||
processInputByte(tmp);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 (0), Failure (-1)
|
||||
*/
|
||||
qint32 UAVTalk::sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances)
|
||||
{
|
||||
return objectTransaction(obj, TYPE_OBJ_REQ, timeout, 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 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
qint32 UAVTalk::sendObject(UAVObject* obj, bool acked, qint32 timeoutMs)
|
||||
{
|
||||
if (acked)
|
||||
{
|
||||
return objectTransaction(obj, TYPE_OBJ_ACK, timeoutMs, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
return objectTransaction(obj, TYPE_OBJ, timeoutMs, false);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute the requested transaction on an object.
|
||||
* \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
|
||||
* \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 respReceived;
|
||||
|
||||
// Lock
|
||||
mutex->lock();
|
||||
// Send object depending on if a response is needed
|
||||
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ)
|
||||
{
|
||||
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
|
||||
respSema->tryAcquire(); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema)
|
||||
respReceived = respSema->tryAcquire(1, timeoutMs); // lock on object until a response is received (or timeout)
|
||||
return respReceived;
|
||||
}
|
||||
else
|
||||
{
|
||||
mutex->unlock();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (type == TYPE_OBJ)
|
||||
{
|
||||
bool success = transmitObject(obj, TYPE_OBJ, allInstances);
|
||||
mutex->unlock();
|
||||
return success;
|
||||
}
|
||||
else
|
||||
{
|
||||
mutex->unlock();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Process an byte from the telemetry stream.
|
||||
* \param[in] rxbyte Received byte
|
||||
* \return Success (true), Failure (false)
|
||||
*/
|
||||
bool UAVTalk::processInputByte(quint8 rxbyte)
|
||||
{
|
||||
// Receive state machine
|
||||
switch (rxState) {
|
||||
case STATE_SYNC:
|
||||
if ((rxbyte & TYPE_MASK) == TYPE_VER )
|
||||
{
|
||||
rxCS = rxbyte;
|
||||
rxType = rxbyte;
|
||||
rxState = STATE_OBJID;
|
||||
rxCount = 0;
|
||||
}
|
||||
break;
|
||||
case STATE_OBJID:
|
||||
rxTmpBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == 4)
|
||||
{
|
||||
// Search for object, if not found reset state machine
|
||||
rxObjId = (qint32)qFromBigEndian<qint32>(rxTmpBuffer);
|
||||
UAVObject* rxObj = objMngr->getObject(rxObjId);
|
||||
if (rxObj == NULL)
|
||||
{
|
||||
rxState = STATE_SYNC;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Update checksum
|
||||
rxCS = updateChecksum(rxCS, rxTmpBuffer, 4);
|
||||
// Determine data length
|
||||
if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK)
|
||||
{
|
||||
rxLength = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
rxLength = rxObj->getNumBytes();
|
||||
}
|
||||
// Check length and determine next state
|
||||
if (rxLength >= MAX_PAYLOAD_LENGTH)
|
||||
{
|
||||
rxState = STATE_SYNC;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Check if this is a single instance object (i.e. if the instance ID field is coming next)
|
||||
if ( rxObj->isSingleInstance() )
|
||||
{
|
||||
// If there is a payload get it, otherwise receive checksum
|
||||
if (rxLength > 0)
|
||||
{
|
||||
rxState = STATE_DATA;
|
||||
}
|
||||
else
|
||||
{
|
||||
rxState = STATE_CS;
|
||||
}
|
||||
rxInstId = 0;
|
||||
rxCount = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
rxState = STATE_INSTID;
|
||||
rxCount = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATE_INSTID:
|
||||
rxTmpBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == 2)
|
||||
{
|
||||
rxInstId = (qint16)qFromBigEndian<qint16>(rxTmpBuffer);
|
||||
rxCS = updateChecksum(rxCS, rxTmpBuffer, 2);
|
||||
rxCount = 0;
|
||||
// If there is a payload get it, otherwise receive checksum
|
||||
if (rxLength > 0)
|
||||
{
|
||||
rxState = STATE_DATA;
|
||||
}
|
||||
else
|
||||
{
|
||||
rxState = STATE_CS;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATE_DATA:
|
||||
rxBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == rxLength)
|
||||
{
|
||||
rxCS = updateChecksum(rxCS, rxBuffer, rxLength);
|
||||
rxState = STATE_CS;
|
||||
rxCount = 0;
|
||||
}
|
||||
break;
|
||||
case STATE_CS:
|
||||
rxTmpBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == 2)
|
||||
{
|
||||
rxCSPacket = (qint16)qFromBigEndian<qint16>(rxTmpBuffer);
|
||||
if (rxCS == rxCSPacket)
|
||||
{
|
||||
mutex->lock();
|
||||
receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength);
|
||||
mutex->unlock();
|
||||
}
|
||||
rxState = STATE_SYNC;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
rxState = STATE_SYNC;
|
||||
}
|
||||
|
||||
// Done
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Receive an object. This function process objects received through the telemetry stream.
|
||||
* \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
|
||||
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
|
||||
* \param[in] data Data buffer
|
||||
* \param[in] length Buffer length
|
||||
* \return Success (true), Failure (false)
|
||||
*/
|
||||
bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* data, qint32 length)
|
||||
{
|
||||
UAVObject* obj = NULL;
|
||||
bool error = false;
|
||||
bool allInstances = (instId == ALL_INSTANCES? true : false);
|
||||
|
||||
// Process message type
|
||||
switch (type) {
|
||||
case TYPE_OBJ:
|
||||
// All instances, not allowed for OBJ messages
|
||||
if (!allInstances)
|
||||
{
|
||||
// Get object and update its data
|
||||
obj = updateObject(objId, instId, data);
|
||||
// Check if an ack is pending
|
||||
updateAck(obj);
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
}
|
||||
break;
|
||||
case TYPE_OBJ_ACK:
|
||||
// All instances, not allowed for OBJ_ACK messages
|
||||
if (!allInstances)
|
||||
{
|
||||
// Get object and update its data
|
||||
obj = updateObject(objId, instId, data);
|
||||
// Transmit ACK
|
||||
transmitObject(obj, TYPE_ACK, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
}
|
||||
break;
|
||||
case TYPE_OBJ_REQ:
|
||||
// Get object, if all instances are requested get instance 0 of the object
|
||||
if (allInstances)
|
||||
{
|
||||
obj = objMngr->getObject(objId);
|
||||
}
|
||||
else
|
||||
{
|
||||
obj = objMngr->getObject(objId, instId);
|
||||
}
|
||||
// If object was found transmit it
|
||||
if (obj != NULL)
|
||||
{
|
||||
transmitObject(obj, TYPE_OBJ, allInstances);
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
}
|
||||
break;
|
||||
case TYPE_ACK:
|
||||
// All instances, not allowed for ACK messages
|
||||
if (!allInstances)
|
||||
{
|
||||
// Get object
|
||||
obj = objMngr->getObject(objId, instId);
|
||||
// Check if an ack is pending
|
||||
if (obj != NULL)
|
||||
{
|
||||
updateAck(obj);
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:
|
||||
error = true;
|
||||
}
|
||||
// Done
|
||||
return !error;
|
||||
}
|
||||
|
||||
|
||||
UAVObject* UAVTalk::updateObject(quint32 objId, quint16 instId, quint8* data)
|
||||
{
|
||||
// Get object
|
||||
UAVObject* obj = objMngr->getObject(objId, instId);
|
||||
// If the instance does not exist create it
|
||||
if (obj == NULL)
|
||||
{
|
||||
obj = objMngr->newObjectInstance(objId, instId);
|
||||
}
|
||||
// Unpack data into object instance
|
||||
obj->unpack(data);
|
||||
return obj;
|
||||
}
|
||||
|
||||
void UAVTalk::updateAck(UAVObject* obj)
|
||||
{
|
||||
if (respObj != NULL && respObj->getObjID() == obj->getObjID() && (respObj->getInstID() == obj->getInstID() || respAllInstances))
|
||||
{
|
||||
respSema->release();
|
||||
respObj = NULL;
|
||||
emit transactionCompleted(obj);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Send an object through the telemetry link.
|
||||
* \param[in] obj Object to send
|
||||
* \param[in] type Transaction type
|
||||
* \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
|
||||
if (allInstances && obj->isSingleInstance())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Process message type
|
||||
if ( type == TYPE_OBJ || type == TYPE_OBJ_ACK )
|
||||
{
|
||||
if (allInstances)
|
||||
{
|
||||
// Get number of instances
|
||||
quint32 numInst = objMngr->getNumInstances(obj->getObjID());
|
||||
// Send all instances
|
||||
for (quint32 instId = 0; instId < numInst; ++instId)
|
||||
{
|
||||
UAVObject* inst = objMngr->getObject(obj->getObjID(), instId);
|
||||
transmitSingleObject(inst, type, false);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return transmitSingleObject(obj, type, false);
|
||||
}
|
||||
}
|
||||
else if (type == TYPE_OBJ_REQ)
|
||||
{
|
||||
return transmitSingleObject(obj, type, allInstances);
|
||||
}
|
||||
else if (type == TYPE_ACK)
|
||||
{
|
||||
if (!allInstances)
|
||||
{
|
||||
return transmitSingleObject(obj, type, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Send an object through the telemetry link.
|
||||
* \param[in] obj Object handle to send
|
||||
* \param[in] type Transaction type
|
||||
* \return Success (true), Failure (false)
|
||||
*/
|
||||
bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances)
|
||||
{
|
||||
qint32 length;
|
||||
qint32 dataOffset;
|
||||
quint16 cs = 0;
|
||||
quint32 objId;
|
||||
quint16 instId;
|
||||
quint16 allInstId = ALL_INSTANCES;
|
||||
|
||||
// Setup type and object id fields
|
||||
objId = obj->getObjID();
|
||||
txBuffer[0] = type;
|
||||
qToBigEndian<qint32>(objId, &txBuffer[1]);
|
||||
|
||||
// Setup instance ID if one is required
|
||||
if ( obj->isSingleInstance() )
|
||||
{
|
||||
dataOffset = 5;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Check if all instances are requested
|
||||
if (allInstances)
|
||||
{
|
||||
qToBigEndian<qint16>(allInstId, &txBuffer[5]);
|
||||
}
|
||||
else
|
||||
{
|
||||
instId = obj->getInstID();
|
||||
qToBigEndian<qint16>(instId, &txBuffer[5]);
|
||||
}
|
||||
dataOffset = 7;
|
||||
}
|
||||
|
||||
// Determine data length
|
||||
if (type == TYPE_OBJ_REQ || type == TYPE_ACK)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#include "uavtalk.h"
|
||||
#include <QtEndian>
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr)
|
||||
{
|
||||
io = iodev;
|
||||
this->objMngr = objMngr;
|
||||
rxState = STATE_SYNC;
|
||||
mutex = new QMutex(QMutex::Recursive);
|
||||
respSema = new QSemaphore(0);
|
||||
respObj = NULL;
|
||||
connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Called each time there are data in the input buffer
|
||||
*/
|
||||
void UAVTalk::processInputStream()
|
||||
{
|
||||
quint8 tmp;
|
||||
|
||||
while (io->bytesAvailable() > 0)
|
||||
{
|
||||
io->read((char*)&tmp, 1);
|
||||
processInputByte(tmp);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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)
|
||||
{
|
||||
return objectTransaction(obj, TYPE_OBJ_REQ, timeout, 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)
|
||||
{
|
||||
if (acked)
|
||||
{
|
||||
return objectTransaction(obj, TYPE_OBJ_ACK, timeoutMs, allInstances);
|
||||
}
|
||||
else
|
||||
{
|
||||
return objectTransaction(obj, TYPE_OBJ, timeoutMs, allInstances);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute the requested transaction on an object.
|
||||
* \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
|
||||
* \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 respReceived;
|
||||
|
||||
// Lock
|
||||
mutex->lock();
|
||||
// Send object depending on if a response is needed
|
||||
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ)
|
||||
{
|
||||
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
|
||||
respSema->tryAcquire(); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema)
|
||||
respReceived = respSema->tryAcquire(1, timeoutMs); // lock on object until a response is received (or timeout)
|
||||
return respReceived;
|
||||
}
|
||||
else
|
||||
{
|
||||
mutex->unlock();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (type == TYPE_OBJ)
|
||||
{
|
||||
bool success = transmitObject(obj, TYPE_OBJ, allInstances);
|
||||
mutex->unlock();
|
||||
return success;
|
||||
}
|
||||
else
|
||||
{
|
||||
mutex->unlock();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Process an byte from the telemetry stream.
|
||||
* \param[in] rxbyte Received byte
|
||||
* \return Success (true), Failure (false)
|
||||
*/
|
||||
bool UAVTalk::processInputByte(quint8 rxbyte)
|
||||
{
|
||||
// Receive state machine
|
||||
switch (rxState) {
|
||||
case STATE_SYNC:
|
||||
if ((rxbyte & TYPE_MASK) == TYPE_VER )
|
||||
{
|
||||
rxCS = rxbyte;
|
||||
rxType = rxbyte;
|
||||
rxState = STATE_OBJID;
|
||||
rxCount = 0;
|
||||
}
|
||||
break;
|
||||
case STATE_OBJID:
|
||||
rxTmpBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == 4)
|
||||
{
|
||||
// Search for object, if not found reset state machine
|
||||
rxObjId = (qint32)qFromBigEndian<qint32>(rxTmpBuffer);
|
||||
UAVObject* rxObj = objMngr->getObject(rxObjId);
|
||||
if (rxObj == NULL)
|
||||
{
|
||||
rxState = STATE_SYNC;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Update checksum
|
||||
rxCS = updateChecksum(rxCS, rxTmpBuffer, 4);
|
||||
// Determine data length
|
||||
if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK)
|
||||
{
|
||||
rxLength = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
rxLength = rxObj->getNumBytes();
|
||||
}
|
||||
// Check length and determine next state
|
||||
if (rxLength >= MAX_PAYLOAD_LENGTH)
|
||||
{
|
||||
rxState = STATE_SYNC;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Check if this is a single instance object (i.e. if the instance ID field is coming next)
|
||||
if ( rxObj->isSingleInstance() )
|
||||
{
|
||||
// If there is a payload get it, otherwise receive checksum
|
||||
if (rxLength > 0)
|
||||
{
|
||||
rxState = STATE_DATA;
|
||||
}
|
||||
else
|
||||
{
|
||||
rxState = STATE_CS;
|
||||
}
|
||||
rxInstId = 0;
|
||||
rxCount = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
rxState = STATE_INSTID;
|
||||
rxCount = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATE_INSTID:
|
||||
rxTmpBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == 2)
|
||||
{
|
||||
rxInstId = (qint16)qFromBigEndian<qint16>(rxTmpBuffer);
|
||||
rxCS = updateChecksum(rxCS, rxTmpBuffer, 2);
|
||||
rxCount = 0;
|
||||
// If there is a payload get it, otherwise receive checksum
|
||||
if (rxLength > 0)
|
||||
{
|
||||
rxState = STATE_DATA;
|
||||
}
|
||||
else
|
||||
{
|
||||
rxState = STATE_CS;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATE_DATA:
|
||||
rxBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == rxLength)
|
||||
{
|
||||
rxCS = updateChecksum(rxCS, rxBuffer, rxLength);
|
||||
rxState = STATE_CS;
|
||||
rxCount = 0;
|
||||
}
|
||||
break;
|
||||
case STATE_CS:
|
||||
rxTmpBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == 2)
|
||||
{
|
||||
rxCSPacket = (qint16)qFromBigEndian<qint16>(rxTmpBuffer);
|
||||
if (rxCS == rxCSPacket)
|
||||
{
|
||||
mutex->lock();
|
||||
receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength);
|
||||
mutex->unlock();
|
||||
}
|
||||
rxState = STATE_SYNC;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
rxState = STATE_SYNC;
|
||||
}
|
||||
|
||||
// Done
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Receive an object. This function process objects received through the telemetry stream.
|
||||
* \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
|
||||
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
|
||||
* \param[in] data Data buffer
|
||||
* \param[in] length Buffer length
|
||||
* \return Success (true), Failure (false)
|
||||
*/
|
||||
bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* data, qint32 length)
|
||||
{
|
||||
UAVObject* obj = NULL;
|
||||
bool error = false;
|
||||
bool allInstances = (instId == ALL_INSTANCES? true : false);
|
||||
|
||||
// Process message type
|
||||
switch (type) {
|
||||
case TYPE_OBJ:
|
||||
// All instances, not allowed for OBJ messages
|
||||
if (!allInstances)
|
||||
{
|
||||
// Get object and update its data
|
||||
obj = updateObject(objId, instId, data);
|
||||
// Check if an ack is pending
|
||||
if ( obj != NULL )
|
||||
{
|
||||
updateAck(obj);
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
}
|
||||
break;
|
||||
case TYPE_OBJ_ACK:
|
||||
// All instances, not allowed for OBJ_ACK messages
|
||||
if (!allInstances)
|
||||
{
|
||||
// Get object and update its data
|
||||
obj = updateObject(objId, instId, data);
|
||||
// Transmit ACK
|
||||
if ( obj != NULL )
|
||||
{
|
||||
transmitObject(obj, TYPE_ACK, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
}
|
||||
break;
|
||||
case TYPE_OBJ_REQ:
|
||||
// Get object, if all instances are requested get instance 0 of the object
|
||||
if (allInstances)
|
||||
{
|
||||
obj = objMngr->getObject(objId);
|
||||
}
|
||||
else
|
||||
{
|
||||
obj = objMngr->getObject(objId, instId);
|
||||
}
|
||||
// If object was found transmit it
|
||||
if (obj != NULL)
|
||||
{
|
||||
transmitObject(obj, TYPE_OBJ, allInstances);
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
}
|
||||
break;
|
||||
case TYPE_ACK:
|
||||
// All instances, not allowed for ACK messages
|
||||
if (!allInstances)
|
||||
{
|
||||
// Get object
|
||||
obj = objMngr->getObject(objId, instId);
|
||||
// Check if an ack is pending
|
||||
if (obj != NULL)
|
||||
{
|
||||
updateAck(obj);
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:
|
||||
error = true;
|
||||
}
|
||||
// Done
|
||||
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
|
||||
UAVObject* obj = objMngr->getObject(objId, instId);
|
||||
// If the instance does not exist create it
|
||||
if (obj == NULL)
|
||||
{
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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))
|
||||
{
|
||||
respSema->release();
|
||||
respObj = NULL;
|
||||
emit transactionCompleted(obj);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Send an object through the telemetry link.
|
||||
* \param[in] obj Object to send
|
||||
* \param[in] type Transaction type
|
||||
* \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
|
||||
if (allInstances && obj->isSingleInstance())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Process message type
|
||||
if ( type == TYPE_OBJ || type == TYPE_OBJ_ACK )
|
||||
{
|
||||
if (allInstances)
|
||||
{
|
||||
// Get number of instances
|
||||
quint32 numInst = objMngr->getNumInstances(obj->getObjID());
|
||||
// Send all instances
|
||||
for (quint32 instId = 0; instId < numInst; ++instId)
|
||||
{
|
||||
UAVObject* inst = objMngr->getObject(obj->getObjID(), instId);
|
||||
transmitSingleObject(inst, type, false);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return transmitSingleObject(obj, type, false);
|
||||
}
|
||||
}
|
||||
else if (type == TYPE_OBJ_REQ)
|
||||
{
|
||||
return transmitSingleObject(obj, TYPE_OBJ_REQ, allInstances);
|
||||
}
|
||||
else if (type == TYPE_ACK)
|
||||
{
|
||||
if (!allInstances)
|
||||
{
|
||||
return transmitSingleObject(obj, TYPE_ACK, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Send an object through the telemetry link.
|
||||
* \param[in] obj Object handle to send
|
||||
* \param[in] type Transaction type
|
||||
* \return Success (true), Failure (false)
|
||||
*/
|
||||
bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances)
|
||||
{
|
||||
qint32 length;
|
||||
qint32 dataOffset;
|
||||
quint16 cs = 0;
|
||||
quint32 objId;
|
||||
quint16 instId;
|
||||
quint16 allInstId = ALL_INSTANCES;
|
||||
|
||||
// Setup type and object id fields
|
||||
objId = obj->getObjID();
|
||||
txBuffer[0] = type;
|
||||
qToBigEndian<qint32>(objId, &txBuffer[1]);
|
||||
|
||||
// Setup instance ID if one is required
|
||||
if ( obj->isSingleInstance() )
|
||||
{
|
||||
dataOffset = 5;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Check if all instances are requested
|
||||
if (allInstances)
|
||||
{
|
||||
qToBigEndian<qint16>(allInstId, &txBuffer[5]);
|
||||
}
|
||||
else
|
||||
{
|
||||
instId = obj->getInstID();
|
||||
qToBigEndian<qint16>(instId, &txBuffer[5]);
|
||||
}
|
||||
dataOffset = 7;
|
||||
}
|
||||
|
||||
// Determine data length
|
||||
if (type == TYPE_OBJ_REQ || type == TYPE_ACK)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -25,76 +25,76 @@
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef UAVTALK_H
|
||||
#define UAVTALK_H
|
||||
|
||||
#include <QIODevice>
|
||||
#include <QMutex>
|
||||
#include <QMutexLocker>
|
||||
#include <QSemaphore>
|
||||
#include "uavobjects/uavobjectmanager.h"
|
||||
|
||||
class UAVTalk: public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr);
|
||||
|
||||
qint32 sendObject(UAVObject* obj, bool acked, qint32 timeoutMs);
|
||||
qint32 sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances);
|
||||
|
||||
signals:
|
||||
void transactionCompleted(UAVObject* obj);
|
||||
|
||||
private slots:
|
||||
void processInputStream(void);
|
||||
|
||||
private:
|
||||
// Constants
|
||||
static const int TYPE_MASK = 0xFC;
|
||||
static const int TYPE_VER = 0x10;
|
||||
static const int TYPE_OBJ = (TYPE_VER | 0x00);
|
||||
static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01);
|
||||
static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02);
|
||||
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 CHECKSUM_LENGTH = 2;
|
||||
static const int MAX_PAYLOAD_LENGTH = 256;
|
||||
static const int MAX_PACKET_LENGTH = (HEADER_LENGTH+MAX_PAYLOAD_LENGTH+CHECKSUM_LENGTH);
|
||||
static const quint16 ALL_INSTANCES = 0xFFFF;
|
||||
|
||||
typedef enum {STATE_SYNC, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxStateType;
|
||||
|
||||
// Variables
|
||||
QIODevice* io;
|
||||
UAVObjectManager* objMngr;
|
||||
QMutex* mutex;
|
||||
QSemaphore* respSema;
|
||||
UAVObject* respObj;
|
||||
bool respAllInstances;
|
||||
quint8 rxBuffer[MAX_PACKET_LENGTH];
|
||||
quint8 txBuffer[MAX_PACKET_LENGTH];
|
||||
// Variables used by the receive state machine
|
||||
quint8 rxTmpBuffer[4];
|
||||
quint8 rxType;
|
||||
quint32 rxObjId;
|
||||
quint16 rxInstId;
|
||||
quint8 rxLength;
|
||||
quint16 rxCSPacket, rxCS;
|
||||
qint32 rxCount;
|
||||
RxStateType rxState;
|
||||
|
||||
// Methods
|
||||
bool objectTransaction(UAVObject* obj, quint8 type, qint32 timeoutMs, 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);
|
||||
void updateAck(UAVObject* obj);
|
||||
bool transmitObject(UAVObject* obj, quint8 type, bool allInstances);
|
||||
bool transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances);
|
||||
quint16 updateChecksum(quint16 cs, quint8* data, qint32 length);
|
||||
|
||||
};
|
||||
|
||||
#endif // UAVTALK_H
|
||||
#ifndef UAVTALK_H
|
||||
#define UAVTALK_H
|
||||
|
||||
#include <QIODevice>
|
||||
#include <QMutex>
|
||||
#include <QMutexLocker>
|
||||
#include <QSemaphore>
|
||||
#include "uavobjects/uavobjectmanager.h"
|
||||
|
||||
class UAVTalk: public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr);
|
||||
|
||||
bool sendObject(UAVObject* obj, bool acked, qint32 timeoutMs, bool allInstances);
|
||||
bool sendObjectRequest(UAVObject* obj, qint32 timeout, bool allInstances);
|
||||
|
||||
signals:
|
||||
void transactionCompleted(UAVObject* obj);
|
||||
|
||||
private slots:
|
||||
void processInputStream(void);
|
||||
|
||||
private:
|
||||
// Constants
|
||||
static const int TYPE_MASK = 0xFC;
|
||||
static const int TYPE_VER = 0x10;
|
||||
static const int TYPE_OBJ = (TYPE_VER | 0x00);
|
||||
static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01);
|
||||
static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02);
|
||||
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 CHECKSUM_LENGTH = 2;
|
||||
static const int MAX_PAYLOAD_LENGTH = 256;
|
||||
static const int MAX_PACKET_LENGTH = (HEADER_LENGTH+MAX_PAYLOAD_LENGTH+CHECKSUM_LENGTH);
|
||||
static const quint16 ALL_INSTANCES = 0xFFFF;
|
||||
|
||||
typedef enum {STATE_SYNC, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxStateType;
|
||||
|
||||
// Variables
|
||||
QIODevice* io;
|
||||
UAVObjectManager* objMngr;
|
||||
QMutex* mutex;
|
||||
QSemaphore* respSema;
|
||||
UAVObject* respObj;
|
||||
bool respAllInstances;
|
||||
quint8 rxBuffer[MAX_PACKET_LENGTH];
|
||||
quint8 txBuffer[MAX_PACKET_LENGTH];
|
||||
// Variables used by the receive state machine
|
||||
quint8 rxTmpBuffer[4];
|
||||
quint8 rxType;
|
||||
quint32 rxObjId;
|
||||
quint16 rxInstId;
|
||||
quint8 rxLength;
|
||||
quint16 rxCSPacket, rxCS;
|
||||
qint32 rxCount;
|
||||
RxStateType rxState;
|
||||
|
||||
// Methods
|
||||
bool objectTransaction(UAVObject* obj, quint8 type, qint32 timeoutMs, 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);
|
||||
void updateAck(UAVObject* obj);
|
||||
bool transmitObject(UAVObject* obj, quint8 type, bool allInstances);
|
||||
bool transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances);
|
||||
quint16 updateChecksum(quint16 cs, quint8* data, qint32 length);
|
||||
|
||||
};
|
||||
|
||||
#endif // UAVTALK_H
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user