modeluavoproxy.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/>
 */

#include <QDebug>
#include <QEventLoop>
#include <QTimer>
#include "modeluavoproxy.h"
#include "extensionsystem/pluginmanager.h"
#include <math.h>

#include "utils/coordinateconversions.h"
#include "homelocation.h"

ModelUavoProxy::ModelUavoProxy(QObject *parent, FlightDataModel *model)
    : QObject(parent)
    , myModel(model)
{
    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
    Q_ASSERT(pm != NULL);
    objManager = pm->getObject<UAVObjectManager>();
    Q_ASSERT(objManager != NULL);
    waypointObj = Waypoint::GetInstance(objManager);
    Q_ASSERT(waypointObj != NULL);
}

bool ModelUavoProxy::modelToObjects()
{
    Waypoint *wp = Waypoint::GetInstance(objManager, 0);
    Q_ASSERT(wp);
    if (wp == NULL)
        return false;

    // Make sure the object is acked
    UAVObject::Metadata initialMeta = wp->getMetadata();
    UAVObject::Metadata meta = initialMeta;
    UAVObject::SetFlightTelemetryAcked(meta, true);
    wp->setMetadata(meta);

    double homeLLA[3];
    double NED[3];
    double LLA[3];
    getHomeLocation(homeLLA);
    bool newInstance;

    // Get the number of existing waypoints
    int instances = Waypoint::getNumInstances(objManager);
    int x;

    // Figure out the total amount of work to do.
    int progressMax = myModel->rowCount();

    if (instances > progressMax) {
        progressMax = instances;
    }

    for (x = 0; x < myModel->rowCount(); x++) {
        newInstance = false;
        Waypoint *wp = NULL; // Shadows above wp

        // Create new instances of waypoints if this is more than exist
        if (x >= instances) {
            newInstance = true;
            wp = new Waypoint;
            wp->initialize(x, wp->getMetaObject());
            objManager->registerObject(wp);
        } else {
            wp = Waypoint::GetInstance(objManager, x);
        }

        Q_ASSERT(wp);
        Waypoint::DataFields waypoint = wp->getData();

        // Convert from LLA to NED for sending to the model
        LLA[0] = myModel->data(myModel->index(x, FlightDataModel::LATPOSITION)).toDouble();
        LLA[1] = myModel->data(myModel->index(x, FlightDataModel::LNGPOSITION)).toDouble();
        LLA[2] = myModel->data(myModel->index(x, FlightDataModel::ALTITUDE)).toDouble();
        Utils::CoordinateConversions().LLA2NED_HomeLLA(LLA, homeLLA, NED);

        // Fetch the data from the internal model
        waypoint.Velocity = myModel->data(myModel->index(x, FlightDataModel::VELOCITY)).toFloat();
        waypoint.Position[Waypoint::POSITION_NORTH] = NED[0];
        waypoint.Position[Waypoint::POSITION_EAST] = NED[1];
        waypoint.Position[Waypoint::POSITION_DOWN] = NED[2];
        waypoint.Mode =
            myModel->data(myModel->index(x, FlightDataModel::MODE), Qt::UserRole).toInt();
        waypoint.ModeParameters =
            myModel->data(myModel->index(x, FlightDataModel::MODE_PARAMS)).toFloat();

        if (robustUpdate(waypoint, x)) {
            qDebug() << "Successfully updated";
            emit sendPathPlanToUavProgress(100 * (x + 1) / progressMax);
        } else {
            qDebug() << "Upload failed";
            emit sendPathPlanToUavProgress(100 * (x + 1) / progressMax);
            return false;
        }
        if (newInstance) {
            QEventLoop m_eventloop;
            QTimer::singleShot(800, &m_eventloop, &QEventLoop::quit);
            m_eventloop.exec();
        }
    }

    /* Continue iterating over any instance indices that aren't needed to
     * represent our model, and mark them invalid.  */
    for (; x < instances; x++) {
        Waypoint *wp = Waypoint::GetInstance(objManager, x);
        // shadows wp above

        Waypoint::DataFields waypoint = wp->getData();

        waypoint.Mode = Waypoint::MODE_INVALID;
        if (robustUpdate(waypoint, x)) {
            qDebug() << "Successfully updated";
            emit sendPathPlanToUavProgress(100 * (x + 1) / progressMax);
        } else {
            qDebug() << "Upload failed";
            emit sendPathPlanToUavProgress(100 * (x + 1) / progressMax);
            return false;
        }
    }

    wp->setMetadata(initialMeta);
    return true;
}

bool ModelUavoProxy::robustUpdate(Waypoint::DataFields data, int instance)
{
    Waypoint *wp = Waypoint::GetInstance(objManager, instance);
    connect(wp, QOverload<UAVObject *, bool>::of(&Waypoint::transactionCompleted), this,
            &ModelUavoProxy::waypointTransactionCompleted);

    for (int i = 0; i < 10; i++) {
        QEventLoop m_eventloop;
        QTimer::singleShot(1000, &m_eventloop, &QEventLoop::quit);
        connect(this, &ModelUavoProxy::waypointTransactionSucceeded, &m_eventloop,
                &QEventLoop::quit);
        connect(this, &ModelUavoProxy::waypointTransactionFailed, &m_eventloop, &QEventLoop::quit);
        waypointTransactionResult.insert(instance, false);
        wp->setData(data);
        wp->updated();
        m_eventloop.exec();
        if (waypointTransactionResult.value(instance)) {
            disconnect(wp, QOverload<UAVObject *, bool>::of(&Waypoint::transactionCompleted), this,
                       &ModelUavoProxy::waypointTransactionCompleted);
            return true;
        }

        // Wait a second for next attempt
        QTimer::singleShot(500, &m_eventloop, &QEventLoop::quit);
        m_eventloop.exec();
    }

    disconnect(wp, QOverload<UAVObject *, bool>::of(&Waypoint::transactionCompleted), this,
               &ModelUavoProxy::waypointTransactionCompleted);

    // None of the attempt got an ack
    return false;
}

void ModelUavoProxy::waypointTransactionCompleted(UAVObject *obj, bool success)
{
    Q_ASSERT(obj->getObjID() == Waypoint::OBJID);
    waypointTransactionResult.insert(obj->getInstID(), success);
    if (success) {
        qDebug() << "Success " << obj->getInstID();
        emit waypointTransactionSucceeded();
    } else {
        qDebug() << "Failed transaction " << obj->getInstID();
        emit waypointTransactionFailed();
    }
}

void ModelUavoProxy::objectsToModel()
{
    double homeLLA[3];
    getHomeLocation(homeLLA);
    double LLA[3];

    myModel->pauseValidation(true);

    myModel->removeRows(0, myModel->rowCount());
    for (int x = 0; x < Waypoint::getNumInstances(objManager); ++x) {
        Waypoint *wp;
        Waypoint::DataFields wpfields;

        wp = Waypoint::GetInstance(objManager, x);
        Q_ASSERT(wp);

        if (!wp)
            continue;

        // Get the waypoint data from the object manager and prepare a row in the internal model
        wpfields = wp->getData();

        if (wpfields.Mode == Waypoint::MODE_INVALID)
            break;

        myModel->insertRow(x);

        // Compute the coordinates in LLA
        double NED[3] = { wpfields.Position[Waypoint::POSITION_NORTH],
                          wpfields.Position[Waypoint::POSITION_EAST],
                          wpfields.Position[Waypoint::POSITION_DOWN] };
        Utils::CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA);

        // Store the data
        myModel->setData(myModel->index(x, FlightDataModel::LATPOSITION), LLA[0]);
        myModel->setData(myModel->index(x, FlightDataModel::LNGPOSITION), LLA[1]);
        myModel->setData(myModel->index(x, FlightDataModel::ALTITUDE), LLA[2]);
        myModel->setData(myModel->index(x, FlightDataModel::VELOCITY), wpfields.Velocity);
        myModel->setData(myModel->index(x, FlightDataModel::MODE), wpfields.Mode);
        myModel->setData(myModel->index(x, FlightDataModel::MODE_PARAMS), wpfields.ModeParameters);
    }

    myModel->pauseValidation(false);
}

bool ModelUavoProxy::getHomeLocation(double *homeLLA)
{
    // Compute the coordinates in LLA
    HomeLocation *home = HomeLocation::GetInstance(objManager);
    if (home == NULL)
        return false;

    HomeLocation::DataFields homeLocation = home->getData();
    homeLLA[0] = homeLocation.Latitude / 1e7;
    homeLLA[1] = homeLocation.Longitude / 1e7;
    homeLLA[2] = homeLocation.Altitude;

    return true;
}