LibrePilot/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp

/**
 ******************************************************************************
 *
 * @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 "pipxsettings.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() != PipXSettings::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 ) ) )
    {
        UAVObject::Metadata metadata = objInfo.obj->getMetadata();
	ObjectTransactionInfo *transInfo = new ObjectTransactionInfo();
        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()
{
    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)
        telem->transactionTimeout(this);
}