1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-14 21:23:52 +01:00
LibrePilot/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp

545 lines
18 KiB
C++

/**
******************************************************************************
*
* @file telemetry.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVTalkPlugin UAVTalk Plugin
* @{
* @brief The UAVTalk protocol plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* 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 "qxtlogger.h"
#include "oplinksettings.h"
#include "objectpersistence.h"
#include <QTime>
#include <QtGlobal>
#include <stdlib.h>
#include <QDebug>
/**
* 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 *)));
connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *)));
// Listen to transaction completions
connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool)));
// Get GCS stats object
gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr);
// Setup and start the periodic timer
timeToNextUpdateMs = 0;
updateTimer = new QTimer(this);
connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates()));
updateTimer->start(1000);
// Setup and start the stats timer
txErrors = 0;
txRetries = 0;
}
Telemetry::~Telemetry()
{
for (QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.begin(); itr != transMap.end(); ++itr) {
delete itr.value();
}
}
/**
* 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, EV_NONE);
}
/**
* 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 = quint32((float)periodMs * (float)qrand() / (float)RAND_MAX); // avoid bunching of updates
}
}
}
/**
* 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 *)));
}
if ((eventMask & EV_UPDATED) != 0) {
connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *)));
}
if ((eventMask & EV_UPDATED_MANUAL) != 0) {
connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject *)), this, SLOT(objectUpdatedManual(UAVObject *)));
}
if ((eventMask & EV_UPDATED_PERIODIC) != 0) {
connect(objs[n], SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *)));
}
if ((eventMask & EV_UPDATE_REQ) != 0) {
connect(objs[n], SIGNAL(updateRequested(UAVObject *)), this, SLOT(updateRequested(UAVObject *)));
}
}
}
/**
* Update an object based on its metadata properties
*/
void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
{
// Get metadata
UAVObject::Metadata metadata = obj->getMetadata();
UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata);
// Setup object depending on update mode
qint32 eventMask;
if (updateMode == UAVObject::UPDATEMODE_PERIODIC) {
// Set update period
setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
// Connect signals for all instances
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
}
connectToObjectInstances(obj, eventMask);
} else if (updateMode == 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 (updateMode == UAVObject::UPDATEMODE_THROTTLED) {
// If we received a periodic update, we can change back to update on change
if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) {
// Set update period
if (eventType == EV_NONE) {
setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
}
// Connect signals for all instances
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
} else {
// Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
// 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 (updateMode == 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);
}
}
/**
* Called when a transaction is successfully completed (uavtalk event)
*/
void Telemetry::transactionCompleted(UAVObject *obj, bool success)
{
// Lookup the transaction in the transaction map.
quint32 objId = obj->getObjID();
QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.find(objId);
if (itr != transMap.end()) {
ObjectTransactionInfo *transInfo = itr.value();
// Remove this transaction as it's complete.
transInfo->timer->stop();
transMap.remove(objId);
delete transInfo;
// Send signal
obj->emitTransactionCompleted(success);
// Process new object updates from queue
processObjectQueue();
} else {
qDebug() << "Error: received a transaction completed when did not expect it.";
}
}
/**
* Called when a transaction is not completed within the timeout period (timer event)
*/
void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
{
transInfo->timer->stop();
// Check if more retries are pending
if (transInfo->retriesRemaining > 0) {
--transInfo->retriesRemaining;
processObjectTransaction(transInfo);
++txRetries;
} else {
// Stop the timer.
transInfo->timer->stop();
// Terminate transaction
utalk->cancelTransaction(transInfo->obj);
// Send signal
transInfo->obj->emitTransactionCompleted(false);
// Remove this transaction as it's complete.
transMap.remove(transInfo->obj->getObjID());
delete transInfo;
// Process new object updates from queue
processObjectQueue();
++txErrors;
}
}
/**
* Start an object transaction with UAVTalk, all information is stored in transInfo
*/
void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo)
{
// Initiate transaction
if (transInfo->objRequest) {
utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances);
} else {
utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances);
}
// Start timer if a response is expected
if (transInfo->objRequest || transInfo->acked) {
transInfo->timer->start(REQ_TIMEOUT_MS);
} else {
// Otherwise, remove this transaction as it's complete.
transMap.remove(transInfo->obj->getObjID());
delete transInfo;
}
}
/**
* Process the event received from an object
*/
void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority)
{
// Push event into queue
ObjectQueueInfo objInfo;
objInfo.obj = obj;
objInfo.event = event;
objInfo.allInstances = allInstances;
if (priority) {
if (objPriorityQueue.length() < MAX_QUEUE_SIZE) {
objPriorityQueue.enqueue(objInfo);
} else {
++txErrors;
obj->emitTransactionCompleted(false);
qxtLog->warning(tr("Telemetry: priority event queue is full, event lost (%1)").arg(obj->getName()));
}
} else {
if (objQueue.length() < MAX_QUEUE_SIZE) {
objQueue.enqueue(objInfo);
} else {
++txErrors;
obj->emitTransactionCompleted(false);
}
}
// Process the transaction
processObjectQueue();
}
/**
* Process events from the object queue
*/
void Telemetry::processObjectQueue()
{
// Get object information from queue (first the priority and then the regular queue)
ObjectQueueInfo objInfo;
if (!objPriorityQueue.isEmpty()) {
objInfo = objPriorityQueue.dequeue();
} else if (!objQueue.isEmpty()) {
objInfo = objQueue.dequeue();
} else {
return;
}
// Check if a connection has been established, only process GCSTelemetryStats updates
// (used to establish the connection)
GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData();
if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) {
objQueue.clear();
if (objInfo.obj->getObjID() != GCSTelemetryStats::OBJID && objInfo.obj->getObjID() != OPLinkSettings::OBJID && objInfo.obj->getObjID() != ObjectPersistence::OBJID) {
objInfo.obj->emitTransactionCompleted(false);
return;
}
}
// Setup transaction (skip if unpack event)
UAVObject::Metadata metadata = objInfo.obj->getMetadata();
UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata);
if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC) || (updateMode != UAVObject::UPDATEMODE_THROTTLED))) {
QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.find(objInfo.obj->getObjID());
if (itr != transMap.end()) {
qDebug() << "!!!!!! Making request for an object: " << objInfo.obj->getName() << " for which a request is already in progress!!!!!!";
}
UAVObject::Metadata metadata = objInfo.obj->getMetadata();
ObjectTransactionInfo *transInfo = new ObjectTransactionInfo(this);
transInfo->obj = objInfo.obj;
transInfo->allInstances = objInfo.allInstances;
transInfo->retriesRemaining = MAX_RETRIES;
transInfo->acked = UAVObject::GetGcsTelemetryAcked(metadata);
if (objInfo.event == EV_UPDATED || objInfo.event == EV_UPDATED_MANUAL || objInfo.event == EV_UPDATED_PERIODIC) {
transInfo->objRequest = false;
} else if (objInfo.event == EV_UPDATE_REQ) {
transInfo->objRequest = true;
}
transInfo->telem = this;
// Insert the transaction into the transaction map.
transMap.insert(objInfo.obj->getObjID(), transInfo);
processObjectTransaction(transInfo);
}
// If this is a metaobject then make necessary telemetry updates
UAVMetaObject *metaobj = dynamic_cast<UAVMetaObject *>(objInfo.obj);
if (metaobj != NULL) {
updateObject(metaobj->getParentObject(), EV_NONE);
} else if (updateMode != UAVObject::UPDATEMODE_THROTTLED) {
updateObject(objInfo.obj, objInfo.event);
}
// The fact we received an unpacked event does not mean that
// we do not have additional objects still in the queue,
// so we have to reschedule queue processing to make sure they are not
// stuck:
if (objInfo.event == EV_UNPACKED) {
processObjectQueue();
}
}
/**
* Check is any objects are pending for periodic updates
* TODO: Clean-up
*/
void Telemetry::processPeriodicUpdates()
{
QMutexLocker locker(mutex);
// Stop timer
updateTimer->stop();
// Iterate through each object and update its timer, if zero then transmit object.
// Also calculate smallest delay to next update (will be used for setting timeToNextUpdateMs)
qint32 minDelay = MAX_UPDATE_PERIOD_MS;
ObjectTimeInfo *objinfo;
qint32 elapsedMs = 0;
QTime time;
qint32 offset;
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
offset = (-objinfo->timeToNextUpdateMs) % objinfo->updatePeriodMs;
objinfo->timeToNextUpdateMs = objinfo->updatePeriodMs - offset;
// Send object
time.start();
processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, true, false);
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
updateTimer->start(timeToNextUpdateMs);
}
Telemetry::TelemetryStats Telemetry::getStats()
{
QMutexLocker locker(mutex);
// Get UAVTalk stats
UAVTalk::ComStats utalkStats = utalk->getStats();
// Update stats
TelemetryStats stats;
stats.txBytes = utalkStats.txBytes;
stats.rxBytes = utalkStats.rxBytes;
stats.txObjectBytes = utalkStats.txObjectBytes;
stats.rxObjectBytes = utalkStats.rxObjectBytes;
stats.rxObjects = utalkStats.rxObjects;
stats.txObjects = utalkStats.txObjects;
stats.txErrors = utalkStats.txErrors + txErrors;
stats.rxErrors = utalkStats.rxErrors;
stats.txRetries = txRetries;
// Done
return stats;
}
void Telemetry::resetStats()
{
QMutexLocker locker(mutex);
utalk->resetStats();
txErrors = 0;
txRetries = 0;
}
void Telemetry::objectUpdatedAuto(UAVObject *obj)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UPDATED, false, true);
}
void Telemetry::objectUpdatedManual(UAVObject *obj)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UPDATED_MANUAL, false, true);
}
void Telemetry::objectUpdatedPeriodic(UAVObject *obj)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UPDATED_PERIODIC, false, true);
}
void Telemetry::objectUnpacked(UAVObject *obj)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UNPACKED, false, true);
}
void Telemetry::updateRequested(UAVObject *obj)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UPDATE_REQ, false, true);
}
void Telemetry::newObject(UAVObject *obj)
{
QMutexLocker locker(mutex);
registerObject(obj);
}
void Telemetry::newInstance(UAVObject *obj)
{
QMutexLocker locker(mutex);
registerObject(obj);
}
ObjectTransactionInfo::ObjectTransactionInfo(QObject *parent) : QObject(parent)
{
obj = 0;
allInstances = false;
objRequest = false;
retriesRemaining = 0;
acked = false;
telem = 0;
// Setup transaction timer
timer = new QTimer(this);
timer->stop();
connect(timer, SIGNAL(timeout()), this, SLOT(timeout()));
}
ObjectTransactionInfo::~ObjectTransactionInfo()
{
telem = 0;
timer->stop();
delete timer;
}
void ObjectTransactionInfo::timeout()
{
if (!telem.isNull()) {
telem->transactionTimeout(this);
}
}