uavobjectutilmanager.cpp

Go to the documentation of this file.

/*
 * 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, see <http://www.gnu.org/licenses/>
 *
 * Additional note on redistribution: The copyright and license notices above
 * must be maintained in each individual source file that is a derivative work
 * of this source file; otherwise redistribution is prohibited.
 */

#include "uavobjectutilmanager.h"
#include "physical_constants.h"
#include "utils/homelocationutil.h"

#include <QDebug>
#include <QEventLoop>
#include <QTimer>
#include <QRegularExpression>
#include <objectpersistence.h>

#include "firmwareiapobj.h"
#include "homelocation.h"
#include "gpsposition.h"
#include "actuatorsettings.h"

#include <coreplugin/coreconstants.h>

#ifdef UAVOBJECTUTIL_DEBUG
#define UAVOBJECTUTIL_QXTLOG_DEBUG(...) qDebug() << __VA_ARGS__
#else // UAVOBJECTUTIL_DEBUG
#define UAVOBJECTUTIL_QXTLOG_DEBUG(...)
#endif // UAVOBJECTUTIL_DEBUG

UAVObjectUtilManager::UAVObjectUtilManager()
{
    saveState = IDLE;
    failureTimer.stop();
    failureTimer.setSingleShot(true);
    failureTimer.setInterval(1000);
    connect(&failureTimer, SIGNAL(timeout()), this, SLOT(objectPersistenceOperationFailed()));

    obm = NULL;

    pm = ExtensionSystem::PluginManager::instance();
    if (pm) {
        obm = pm->getObject<UAVObjectManager>();
    }
}

UAVObjectUtilManager::~UAVObjectUtilManager()
{
    disconnect();
}

UAVObjectManager *UAVObjectUtilManager::getObjectManager()
{
    Q_ASSERT(obm);
    return obm;
}

// ******************************
// Flash memory saving
//

void UAVObjectUtilManager::saveObjectToFlash(UAVObject *obj)
{
    // Add to queue
    queue.enqueue(obj);
    UAVOBJECTUTIL_QXTLOG_DEBUG(QString("Enqueue object:%0").arg(obj->getName()));

    // If queue length is one, then start sending (call sendNextObject)
    // Otherwise, do nothing, because we are already sending.
    if (queue.length() == 1)
        saveNextObject();
}

void UAVObjectUtilManager::saveNextObject()
{
    if (queue.isEmpty()) {
        return;
    }

    Q_ASSERT(saveState == IDLE);

    // Get next object from the queue (don't dequeue yet)
    UAVObject *obj = queue.head();
    Q_ASSERT(obj);
    UAVOBJECTUTIL_QXTLOG_DEBUG(QString("Send save object request to board %0").arg(obj->getName()));

    ObjectPersistence *objectPersistence = ObjectPersistence::GetInstance(getObjectManager());
    Q_ASSERT(objectPersistence);

    // "transactionCompleted" is emitted once the objectPersistence object is sent over the
    // telemetry link.
    // Since its metadata state that it should be ACK'ed on flight telemetry, the
    // transactionCompleted signal
    // will be triggered once the GCS telemetry manager receives an ACK from the flight controller,
    // or times
    // out. the success value will reflect success or failure.
    connect(objectPersistence, SIGNAL(transactionCompleted(UAVObject *, bool)), this,
            SLOT(objectPersistenceTransactionCompleted(UAVObject *, bool)), Qt::UniqueConnection);
    // After we update the objectPersistence UAVO, we need to listen to its "objectUpdated" event,
    // which will occur
    // once the flight controller sends this object back to the GCS, with the "Operation" field set
    // to "Completed"
    // or "Error".
    connect(objectPersistence, SIGNAL(objectUpdated(UAVObject *)), this,
            SLOT(objectPersistenceUpdated(UAVObject *)), Qt::UniqueConnection);
    saveState = AWAITING_ACK;
    UAVOBJECTUTIL_QXTLOG_DEBUG(QString("[saveObjectToFlash] Moving on to AWAITING_ACK"));

    ObjectPersistence::DataFields data;
    data.Operation = ObjectPersistence::OPERATION_SAVE;
    data.ObjectID = obj->getObjID();
    data.InstanceID = obj->getInstID();
    objectPersistence->setData(data);
    objectPersistence->updated();
    // Now: we are going to get the following:
    // - two "objectUpdated" messages (one coming from GCS, one coming from Flight, which will
    // confirm the object
    //   was properly received by both sides). This is because the metadata of objectPersistence has
    //   "FlightUpdateOnChange"
    //   set to true.
    //  - then one "transactionCompleted" indicating that the Flight side did not only receive the
    //  object but it did
    //    receive it without error (that message will be triggered by reception of the ACK).
    //  - Last we will get one last "objectUpdated" message coming from flight side, where we'll get
    //  the results of
    //    the objectPersistence operation we asked for (saved, other).
}

void UAVObjectUtilManager::objectPersistenceTransactionCompleted(UAVObject *obj, bool success)
{
    if (success) {
        Q_ASSERT(obj->getName().compare("ObjectPersistence") == 0);
        Q_ASSERT(saveState == AWAITING_ACK);
        // Two things can happen then:
        // Either the Object Save Request did actually go through, and then we should get in
        // "AWAITING_COMPLETED" mode, or the Object Save Request did _not_ go through, for example
        // because the object does not exist and then we will never get a subsequent update - the
        // flight firmware
        // does not update the objectPersistence UAVO with "Error", it just ignores the request.
        // For this reason, we will arm a 1 second timer to make provision for this and not block
        // the queue:
        saveState = AWAITING_COMPLETED;
        UAVOBJECTUTIL_QXTLOG_DEBUG(QString("[saveObjectToFlash] Moving on to AWAITING_COMPLETED"));
        disconnect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this,
                   SLOT(objectPersistenceTransactionCompleted(UAVObject *, bool)));
        failureTimer.start(2000); // Create a timeout
    } else {
        // Can be caused by timeout errors on sending.  Forget it and send next.
        UAVOBJECTUTIL_QXTLOG_DEBUG(QString("objectPersistenceTranscationCompleted (error))"));
        ObjectPersistence *objectPersistence = ObjectPersistence::GetInstance(getObjectManager());
        Q_ASSERT(objectPersistence);

        objectPersistence->disconnect(this);
        queue.dequeue(); // We can now remove the object, it failed anyway.
        saveState = IDLE;
        // Careful below: objectPersistence contains a field 'ObjectID' that corresponds to
        // the objectID we wanted to save. Don't confuse with the OBJID of the Objectpersistence
        // UAVO itself!
        emit saveCompleted(objectPersistence->getObjectID(), false);
        saveNextObject();
    }
}

void UAVObjectUtilManager::objectPersistenceOperationFailed()
{
    if (saveState == AWAITING_COMPLETED) {

        ObjectPersistence *objectPersistence = ObjectPersistence::GetInstance(getObjectManager());
        Q_ASSERT(objectPersistence);

        UAVObject *obj = queue.dequeue(); // We can now remove the object, it failed anyway.
        Q_ASSERT(obj);

        objectPersistence->disconnect(this);

        saveState = IDLE;
        emit saveCompleted(obj->getObjID(), false);

        saveNextObject();
    }
}

void UAVObjectUtilManager::objectPersistenceUpdated(UAVObject *obj)
{
    Q_ASSERT(obj);
    Q_ASSERT(obj->getObjID() == ObjectPersistence::OBJID);

    if (saveState != AWAITING_COMPLETED) {
        // We'll get two of those before we're in AWAITING_COMPLETED state...
        return;
    }

    ObjectPersistence::DataFields objectPersistence =
        (static_cast<ObjectPersistence *>(obj))->getData();

    if (objectPersistence.Operation == ObjectPersistence::OPERATION_ERROR) {
        failureTimer.stop();
        objectPersistenceOperationFailed();
    } else if (objectPersistence.Operation == ObjectPersistence::OPERATION_COMPLETED) {
        failureTimer.stop();
        // Check right object saved
        UAVObject *savingObj = queue.head();
        if (objectPersistence.ObjectID != savingObj->getObjID()) {
            objectPersistenceOperationFailed();
            return;
        }

        obj->disconnect(this);
        queue.dequeue(); // We can now remove the object, it's done.
        saveState = IDLE;
        UAVOBJECTUTIL_QXTLOG_DEBUG(QString("[saveObjectToFlash] Object save succeeded"));
        emit saveCompleted(objectPersistence.ObjectID, true);
        saveNextObject();
    }
}

QMap<QString, UAVObject::Metadata> UAVObjectUtilManager::readAllNonSettingsMetadata()
{
    return readMetadata(NONSETTINGS_METADATA_ONLY);
}

QMap<QString, UAVObject::Metadata>
UAVObjectUtilManager::readMetadata(metadataSetEnum metadataReadType)
{
    QMap<QString, UAVObject::Metadata> metaDataList;

    // Save all metadata objects.
    UAVObjectManager *objManager = getObjectManager();
    QVector<QVector<UAVDataObject *>> objList = objManager->getDataObjectsVector();
    foreach (QVector<UAVDataObject *> list, objList) {
        foreach (UAVDataObject *obj, list) {
            bool updateMetadataFlag = false;
            switch (metadataReadType) {
            case ALL_METADATA:
                updateMetadataFlag = true;
                break;
            case SETTINGS_METADATA_ONLY:
                if (obj->isSettings()) {
                    updateMetadataFlag = true;
                }
                break;
            case NONSETTINGS_METADATA_ONLY:
                if (!obj->isSettings()) {
                    updateMetadataFlag = true;
                }
                break;
            }

            if (updateMetadataFlag) {
                metaDataList.insert(obj->getName(), obj->getMetadata());
            }
        }
    }

    return metaDataList;
}

bool UAVObjectUtilManager::setAllNonSettingsMetadata(
    QMap<QString, UAVObject::Metadata> metaDataList)
{
    metadataSetEnum metadataSetType = NONSETTINGS_METADATA_ONLY;
    setMetadata(metaDataList, metadataSetType);

    return true;
}

bool UAVObjectUtilManager::setMetadata(QMap<QString, UAVObject::Metadata> metaDataSetList,
                                       metadataSetEnum metadataSetType)
{
    if (!metadataSendlist.isEmpty())
        return false;
    // Load all metadata objects.
    UAVObjectManager *objManager = getObjectManager();
    foreach (QString str, metaDataSetList.keys()) {
        UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(str));
        bool updateMetadataFlag = false;
        switch (metadataSetType) {
        case ALL_METADATA:
            updateMetadataFlag = true;
            break;
        case SETTINGS_METADATA_ONLY:
            if (obj->isSettings()) {
                updateMetadataFlag = true;
            }
            break;
        case NONSETTINGS_METADATA_ONLY:
            if (!obj->isSettings()) {
                updateMetadataFlag = true;
            }
            break;
        }

        // Only enqueue objects that are present on hardware. If session
        // management is disabled this will always return true.
        if (updateMetadataFlag && obj->getIsPresentOnHardware()) {
            UAVOBJECTUTIL_QXTLOG_DEBUG(QString("Enqueued %0").arg(obj->getName()));
            metadataSendlist.insert(obj, metaDataSetList.value(str));
        }
    }
    metadataSendSuccess = true;
    if (metadataSendlist.isEmpty()) {
        emit completedMetadataWrite(true);
        return true;
    }
    UAVDataObject *obj = metadataSendlist.keys().first();

    // Connect transaction and timeout timers before making the request
    connect(obj->getMetaObject(), SIGNAL(transactionCompleted(UAVObject *, bool)), this,
            SLOT(metadataTransactionCompleted(UAVObject *, bool)), Qt::UniqueConnection);
    obj->setMetadata(metadataSendlist.value(obj));

    return true;
}

bool UAVObjectUtilManager::resetMetadataToDefaults()
{
    QMap<QString, UAVObject::Metadata> metaDataList;

    // Load all metadata object defaults
    UAVObjectManager *objManager = getObjectManager();
    QVector<QVector<UAVDataObject *>> objList = objManager->getDataObjectsVector();
    foreach (QVector<UAVDataObject *> list, objList) {
        foreach (UAVDataObject *obj, list) {
            metaDataList.insert(obj->getName(), obj->getDefaultMetadata());
        }
    }

    // Save metadata
    metadataSetEnum metadataSetType = ALL_METADATA;
    bool ret = setMetadata(metaDataList, metadataSetType);

    return ret;
}

void UAVObjectUtilManager::metadataTransactionCompleted(UAVObject *uavoObject, bool success)
{
    UAVMetaObject *mobj = dynamic_cast<UAVMetaObject *>(uavoObject);
    UAVDataObject *dobj = dynamic_cast<UAVDataObject *>(mobj->getParentObject());
    Q_ASSERT(mobj);
    Q_ASSERT(dobj);
    if (metadataSendlist.contains(dobj)) {
        if (success) {
            UAVOBJECTUTIL_QXTLOG_DEBUG(
                QString("Writing metadata succeded on %0").arg(uavoObject->getName()));
        } else {
            // If unsuccessful
            metadataSendSuccess = false;
            UAVOBJECTUTIL_QXTLOG_DEBUG(
                QString("metadata send failed").arg(metadataSendlist.keys().first()->getName()));
        }
        disconnect(mobj, SIGNAL(transactionCompleted(UAVObject *, bool)), this,
                   SLOT(metadataTransactionCompleted(UAVObject *, bool)));
        metadataSendlist.take(dobj);
        if (!metadataSendlist.keys().isEmpty()) {
            metadataSendlist.keys().first()->setMetadata(
                metadataSendlist.value(metadataSendlist.keys().first()));
            connect(metadataSendlist.keys().first()->getMetaObject(),
                    SIGNAL(transactionCompleted(UAVObject *, bool)), this,
                    SLOT(metadataTransactionCompleted(UAVObject *, bool)), Qt::UniqueConnection);
            return;
        } else {
            emit completedMetadataWrite(metadataSendSuccess);
        }
    }
}

FirmwareIAPObj::DataFields UAVObjectUtilManager::getFirmwareIap()
{
    FirmwareIAPObj::DataFields dummy = {};

    FirmwareIAPObj *firmwareIap = FirmwareIAPObj::GetInstance(obm);
    Q_ASSERT(firmwareIap);
    if (!firmwareIap)
        return dummy;

    return firmwareIap->getData();
}

int UAVObjectUtilManager::getBoardModel()
{
    FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap();
    int ret = firmwareIapData.BoardType << 8;
    ret = ret + firmwareIapData.BoardRevision;
    return ret;
}

int UAVObjectUtilManager::getBoardRevision()
{
    FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap();
    int ret = firmwareIapData.BoardRevision;
    return ret;
}

Core::IBoardType *UAVObjectUtilManager::getBoardType()
{
    int boardTypeNum = (getBoardModel() >> 8) & 0x00ff;
    QList<Core::IBoardType *> boards = pm->getObjects<Core::IBoardType>();
    foreach (Core::IBoardType *board, boards) {
        if (board->getBoardType() == boardTypeNum)
            return board;
    }
    return NULL;
}

QByteArray UAVObjectUtilManager::getBoardCPUSerial()
{
    QByteArray cpuSerial;
    FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap();

    for (unsigned int i = 0; i < FirmwareIAPObj::CPUSERIAL_NUMELEM; i++)
        cpuSerial.append(firmwareIapData.CPUSerial[i]);

    return cpuSerial;
}

quint32 UAVObjectUtilManager::getFirmwareCRC()
{
    FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap();
    return firmwareIapData.crc;
}

QByteArray UAVObjectUtilManager::getBoardDescription()
{
    QByteArray ret;
    FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap();

    for (unsigned int i = 0; i < FirmwareIAPObj::DESCRIPTION_NUMELEM; i++)
        ret.append(firmwareIapData.Description[i]);

    return ret;
}

// ******************************
// HomeLocation

int UAVObjectUtilManager::setHomeLocation(double LLA[3], bool save_to_sdcard)
{
    double Be[3];

    int retval = Utils::HomeLocationUtil().getDetails(LLA, Be);
    Q_ASSERT(retval >= 0);

    if (retval < 0)
        return -1;

    // ******************
    // save the new settings

    HomeLocation *homeLocation = HomeLocation::GetInstance(obm);
    Q_ASSERT(homeLocation != NULL);

    HomeLocation::DataFields homeLocationData = homeLocation->getData();
    homeLocationData.Latitude = LLA[0] * 1e7;
    homeLocationData.Longitude = LLA[1] * 1e7;
    homeLocationData.Altitude = LLA[2];

    homeLocationData.Be[0] = Be[0];
    homeLocationData.Be[1] = Be[1];
    homeLocationData.Be[2] = Be[2];

    homeLocationData.Set = HomeLocation::SET_TRUE;

    homeLocation->setData(homeLocationData);

    if (save_to_sdcard)
        saveObjectToFlash(homeLocation);

    return 0;
}

int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3])
{
    HomeLocation *homeLocation = HomeLocation::GetInstance(obm);
    Q_ASSERT(homeLocation != NULL);

    HomeLocation::DataFields homeLocationData = homeLocation->getData();

    set = homeLocationData.Set;

    LLA[0] = homeLocationData.Latitude * 1e-7;
    LLA[1] = homeLocationData.Longitude * 1e-7;
    LLA[2] = homeLocationData.Altitude;

    if (LLA[0] != LLA[0])
        LLA[0] = 0; // nan detection
    else if (LLA[0] > 90)
        LLA[0] = 90;
    else if (LLA[0] < -90)
        LLA[0] = -90;

    if (LLA[1] != LLA[1])
        LLA[1] = 0; // nan detection
    else if (LLA[1] > 180)
        LLA[1] = 180;
    else if (LLA[1] < -180)
        LLA[1] = -180;

    if (LLA[2] != LLA[2])
        LLA[2] = 0; // nan detection

    return 0; // OK
}

// ******************************
// GPS

int UAVObjectUtilManager::getGPSPosition(double LLA[3])
{
    GPSPosition *gpsPosition = GPSPosition::GetInstance(obm);
    Q_ASSERT(gpsPosition != NULL);

    GPSPosition::DataFields gpsPositionData = gpsPosition->getData();

    LLA[0] = gpsPositionData.Latitude;
    LLA[1] = gpsPositionData.Longitude;
    LLA[2] = gpsPositionData.Altitude;

    if (LLA[0] != LLA[0])
        LLA[0] = 0; // nan detection
    else if (LLA[0] > 90)
        LLA[0] = 90;
    else if (LLA[0] < -90)
        LLA[0] = -90;

    if (LLA[1] != LLA[1])
        LLA[1] = 0; // nan detection
    else if (LLA[1] > 180)
        LLA[1] = 180;
    else if (LLA[1] < -180)
        LLA[1] = -180;

    if (LLA[2] != LLA[2])
        LLA[2] = 0; // nan detection

    return 0; // OK
}

bool UAVObjectUtilManager::getBoardDescriptionStruct(deviceDescriptorStruct &device)
{
    bool ret = descriptionToStructure(getBoardDescription(), device);
    return ret;
}

bool UAVObjectUtilManager::descriptionToStructure(QByteArray desc, deviceDescriptorStruct &struc)
{
    if (desc.startsWith("TlFw") || desc.startsWith("OpFw")) {
        /*
         * This looks like a binary with a description at the end
         *  4 bytes: header: "TlFw"
         *  4 bytes: git commit hash (short version of SHA1)
         *  4 bytes: Unix timestamp of last git commit
         *  2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in
         * board define files.
         *  26 bytes: commit tag if it is there, otherwise "Unreleased". Zero-padded
         *  20 bytes: SHA1 sum of the firmware.
         *  20 bytes: SHA1 sum of the UAVO definition files.
         *   8 bytes: git commit hash of common ancestor of the firmware with next
         *  12 bytes: user defined string
         */

        // Note: the ARM binary is big-endian:
        quint32 gitCommitHash = desc.at(7) & 0xFF;
        for (int i = 0; i < 4; i++) {
            gitCommitHash = gitCommitHash << 8;
            gitCommitHash += desc.at(7 - i) & 0xFF;
        }
        struc.gitHash = QString("%1").arg(gitCommitHash, 8, 16, QChar('0'));

        quint64 gitAncestorHash = 0;
        for (int i = 0; i < 8; i++) {
            gitAncestorHash = gitAncestorHash << 8;
            gitAncestorHash += desc.at(87 - i) & 0xFF;
        }
        struc.nextAncestor = QString("%1").arg(gitAncestorHash, 16, 16, QChar('0'));

        quint32 gitDate = desc.at(11) & 0xFF;
        for (int i = 1; i < 4; i++) {
            gitDate = gitDate << 8;
            gitDate += desc.at(11 - i) & 0xFF;
        }
        struc.gitDate = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyyMMdd HH:mm");

        QString gitTag = QString(desc.mid(14, 26));
        struc.gitTag = gitTag;

        // TODO: check platform compatibility
        QByteArray targetPlatform = desc.mid(12, 2);
        struc.boardType = (int)targetPlatform.at(0);
        struc.boardRevision = (int)targetPlatform.at(1);
        struc.fwHash = desc.mid(40, 20);
        struc.uavoHash = desc.mid(60, 20);

        // The git tag format used for releases is Release-yyyymmdd, optionally with a suffix
        // .[0-9] to allow multiple tags on the same day e.g. if a bug is discovered immediately
        QRegularExpression certifiedPattern("^Release-20[0-9]{6}(\\.[0-9])?$");
        if (certifiedPattern.match(struc.gitTag).hasMatch())
            struc.certified = true;
        else
            struc.certified = false;
        struc.userDefined = desc.mid(88, 12);

        return true;
    }
    return false;
}

QString UAVObjectUtilManager::getFirmwareHash()
{
    deviceDescriptorStruct dev;
    if (getBoardDescriptionStruct(dev))
        return dev.gitHash;
    return QString();
}

QString UAVObjectUtilManager::getGcsHash()
{
    return QString(Core::Constants::GCS_REVISION_SHORT_STR);
}

bool UAVObjectUtilManager::firmwareHashMatchesGcs()
{
    return getFirmwareHash().toLower() == getGcsHash().toLower();
}

bool UAVObjectUtilManager::boardConfigured()
{
    auto actuatorSettings = ActuatorSettings::GetInstance(obm);
    if (!actuatorSettings)
        return true; // this might seem odd, but matches the API description

    if (!actuatorSettings->getIsPresentOnHardware())
        return true; // Maybe pipx, maybe mismatched fw ver...

    auto minField = actuatorSettings->getField("ChannelMin");
    auto maxField = actuatorSettings->getField("ChannelMax");
    if (!minField || !maxField)
        return true;

    for (auto i = 0; i < maxField->getNumElements(); i++) {
        if (maxField->getValue(i).toInt() > 0)
            return true;
    }

    for (auto i = 0; i < minField->getNumElements(); i++) {
        if (minField->getValue(i).toInt() > 0)
            return true;
    }

    return false;
}

// ******************************