diff --git a/ground/src/plugins/uavtalk/telemetry.cpp b/ground/src/plugins/uavtalk/telemetry.cpp index 04f7b8568..5dbab1ce4 100644 --- a/ground/src/plugins/uavtalk/telemetry.cpp +++ b/ground/src/plugins/uavtalk/telemetry.cpp @@ -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 + +/** + * 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 > 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 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(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(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(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(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); +} + + + + diff --git a/ground/src/plugins/uavtalk/telemetry.h b/ground/src/plugins/uavtalk/telemetry.h index 4e05f6b90..471c3125e 100644 --- a/ground/src/plugins/uavtalk/telemetry.h +++ b/ground/src/plugins/uavtalk/telemetry.h @@ -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 +#include +#include +#include + +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 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 diff --git a/ground/src/plugins/uavtalk/uavtalk.cpp b/ground/src/plugins/uavtalk/uavtalk.cpp index 0fbe85182..68f1c76fd 100644 --- a/ground/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/src/plugins/uavtalk/uavtalk.cpp @@ -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 - -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(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(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(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(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(allInstId, &txBuffer[5]); - } - else - { - instId = obj->getInstID(); - qToBigEndian(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(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 + +/** + * 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(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(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(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(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(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(allInstId, &txBuffer[5]); + } + else + { + instId = obj->getInstID(); + qToBigEndian(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(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; +} + + + + + + + + + + + + + + + + + diff --git a/ground/src/plugins/uavtalk/uavtalk.h b/ground/src/plugins/uavtalk/uavtalk.h index 4bb462980..5fe8b77f6 100644 --- a/ground/src/plugins/uavtalk/uavtalk.h +++ b/ground/src/plugins/uavtalk/uavtalk.h @@ -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 -#include -#include -#include -#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 +#include +#include +#include +#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 diff --git a/ground/src/plugins/uavtalk/uavtalkplugin.cpp b/ground/src/plugins/uavtalk/uavtalkplugin.cpp index 0d35c5e01..c6ae8b85f 100644 --- a/ground/src/plugins/uavtalk/uavtalkplugin.cpp +++ b/ground/src/plugins/uavtalk/uavtalkplugin.cpp @@ -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(); + // 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; } diff --git a/ground/src/plugins/uavtalk/uavtalkplugin.h b/ground/src/plugins/uavtalk/uavtalkplugin.h index 3784c8958..13a3751f7 100644 --- a/ground/src/plugins/uavtalk/uavtalkplugin.h +++ b/ground/src/plugins/uavtalk/uavtalkplugin.h @@ -28,15 +28,15 @@ #ifndef UAVTALKPLUGIN_H #define UAVTALKPLUGIN_H - #include +#include #include +#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