/** ****************************************************************************** * * @file modeluavproxy.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin * @{ * @brief The OpenPilot Map 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 "modeluavoproxy.h" #include "extensionsystem/pluginmanager.h" #include modelUavoProxy::modelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(pm != NULL); objManager = pm->getObject(); Q_ASSERT(objManager != NULL); waypointObj = Waypoint::GetInstance(objManager); Q_ASSERT(waypointObj != NULL); pathactionObj = PathAction::GetInstance(objManager); Q_ASSERT(pathactionObj != NULL); } void modelUavoProxy::modelToObjects() { PathAction *act = NULL; Waypoint *wp = NULL; QModelIndex index; double distance; double bearing; double altitude; int lastaction = -1; for (int x = 0; x < myModel->rowCount(); ++x) { int instances = objManager->getNumInstances(waypointObj->getObjID()); if (x > instances - 1) { wp = new Waypoint; wp->initialize(x, wp->getMetaObject()); objManager->registerObject(wp); } else { wp = Waypoint::GetInstance(objManager, x); } act = new PathAction; Q_ASSERT(act); Q_ASSERT(wp); Waypoint::DataFields waypoint = wp->getData(); PathAction::DataFields action = act->getData(); ///Waypoint object data index = myModel->index(x, flightDataModel::DISRELATIVE); distance = myModel->data(index).toDouble(); index = myModel->index(x, flightDataModel::BEARELATIVE); bearing = myModel->data(index).toDouble(); index = myModel->index(x, flightDataModel::ALTITUDERELATIVE); altitude = myModel->data(index).toFloat(); index = myModel->index(x, flightDataModel::VELOCITY); waypoint.Velocity = myModel->data(index).toFloat(); waypoint.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI); waypoint.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / 180 * M_PI); waypoint.Position[Waypoint::POSITION_DOWN] = (-1.0f) * altitude; ///PathAction object data index = myModel->index(x, flightDataModel::MODE); action.Mode = myModel->data(index).toInt(); index = myModel->index(x, flightDataModel::MODE_PARAMS0); action.ModeParameters[0] = myModel->data(index).toFloat(); index = myModel->index(x, flightDataModel::MODE_PARAMS1); action.ModeParameters[1] = myModel->data(index).toFloat(); index = myModel->index(x, flightDataModel::MODE_PARAMS2); action.ModeParameters[2] = myModel->data(index).toFloat(); index = myModel->index(x, flightDataModel::MODE_PARAMS3); action.ModeParameters[3] = myModel->data(index).toFloat(); index = myModel->index(x, flightDataModel::CONDITION); action.EndCondition = myModel->data(index).toInt(); index = myModel->index(x, flightDataModel::CONDITION_PARAMS0); action.ConditionParameters[0] = myModel->data(index).toFloat(); index = myModel->index(x, flightDataModel::CONDITION_PARAMS1); action.ConditionParameters[1] = myModel->data(index).toFloat(); index = myModel->index(x, flightDataModel::CONDITION_PARAMS2); action.ConditionParameters[2] = myModel->data(index).toFloat(); index = myModel->index(x, flightDataModel::CONDITION_PARAMS3); action.ConditionParameters[3] = myModel->data(index).toFloat(); index = myModel->index(x, flightDataModel::COMMAND); action.Command = myModel->data(index).toInt(); index = myModel->index(x, flightDataModel::JUMPDESTINATION); action.JumpDestination = myModel->data(index).toInt() - 1; index = myModel->index(x, flightDataModel::ERRORDESTINATION); action.ErrorDestination = myModel->data(index).toInt() - 1; int actionNumber = addAction(act, action, lastaction); if (actionNumber > lastaction) { lastaction = actionNumber; } waypoint.Action = actionNumber; wp->setData(waypoint); wp->updated(); } } void modelUavoProxy::objectsToModel() { Waypoint *wp; Waypoint::DataFields wpfields; PathAction *action; QModelIndex index; double distance; double bearing; PathAction::DataFields actionfields; myModel->removeRows(0, myModel->rowCount()); for (int x = 0; x < objManager->getNumInstances(waypointObj->getObjID()); ++x) { wp = Waypoint::GetInstance(objManager, x); Q_ASSERT(wp); if (!wp) { continue; } wpfields = wp->getData(); myModel->insertRow(x); index = myModel->index(x, flightDataModel::VELOCITY); myModel->setData(index, wpfields.Velocity); distance = sqrt(wpfields.Position[Waypoint::POSITION_NORTH] * wpfields.Position[Waypoint::POSITION_NORTH] + wpfields.Position[Waypoint::POSITION_EAST] * wpfields.Position[Waypoint::POSITION_EAST]); bearing = atan2(wpfields.Position[Waypoint::POSITION_EAST], wpfields.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI; if (bearing != bearing) { bearing = 0; } index = myModel->index(x, flightDataModel::DISRELATIVE); myModel->setData(index, distance); index = myModel->index(x, flightDataModel::BEARELATIVE); myModel->setData(index, bearing); index = myModel->index(x, flightDataModel::ALTITUDERELATIVE); myModel->setData(index, (-1.0f) * wpfields.Position[Waypoint::POSITION_DOWN]); action = PathAction::GetInstance(objManager, wpfields.Action); Q_ASSERT(action); if (!action) { continue; } actionfields = action->getData(); index = myModel->index(x, flightDataModel::ISRELATIVE); myModel->setData(index, true); index = myModel->index(x, flightDataModel::COMMAND); myModel->setData(index, actionfields.Command); index = myModel->index(x, flightDataModel::CONDITION_PARAMS0); myModel->setData(index, actionfields.ConditionParameters[0]); index = myModel->index(x, flightDataModel::CONDITION_PARAMS1); myModel->setData(index, actionfields.ConditionParameters[1]); index = myModel->index(x, flightDataModel::CONDITION_PARAMS2); myModel->setData(index, actionfields.ConditionParameters[2]); index = myModel->index(x, flightDataModel::CONDITION_PARAMS3); myModel->setData(index, actionfields.ConditionParameters[3]); index = myModel->index(x, flightDataModel::CONDITION); myModel->setData(index, actionfields.EndCondition); index = myModel->index(x, flightDataModel::ERRORDESTINATION); myModel->setData(index, actionfields.ErrorDestination + 1); index = myModel->index(x, flightDataModel::JUMPDESTINATION); myModel->setData(index, actionfields.JumpDestination + 1); index = myModel->index(x, flightDataModel::MODE); myModel->setData(index, actionfields.Mode); index = myModel->index(x, flightDataModel::MODE_PARAMS0); myModel->setData(index, actionfields.ModeParameters[0]); index = myModel->index(x, flightDataModel::MODE_PARAMS1); myModel->setData(index, actionfields.ModeParameters[1]); index = myModel->index(x, flightDataModel::MODE_PARAMS2); myModel->setData(index, actionfields.ModeParameters[2]); index = myModel->index(x, flightDataModel::MODE_PARAMS3); myModel->setData(index, actionfields.ModeParameters[3]); } } int modelUavoProxy::addAction(PathAction *actionObj, PathAction::DataFields actionFields, int lastaction) { // check if a similar action already exhists int instances = objManager->getNumInstances(pathactionObj->getObjID()); for (int x = 0; x < lastaction + 1; ++x) { PathAction *action = PathAction::GetInstance(objManager, x); Q_ASSERT(action); if (!action) { continue; } PathAction::DataFields fields = action->getData(); if (fields.Command == actionFields.Command && fields.ConditionParameters[0] == actionFields.ConditionParameters[0] && fields.ConditionParameters[1] == actionFields.ConditionParameters[1] && fields.ConditionParameters[2] == actionFields.ConditionParameters[2] && fields.EndCondition == actionFields.EndCondition && fields.ErrorDestination == actionFields.ErrorDestination && fields.JumpDestination == actionFields.JumpDestination && fields.Mode == actionFields.Mode && fields.ModeParameters[0] == actionFields.ModeParameters[0] && fields.ModeParameters[1] == actionFields.ModeParameters[1] && fields.ModeParameters[2] == actionFields.ModeParameters[2]) { qDebug() << "ModelUAVProxy:" << "found similar action instance:" << x; actionObj->deleteLater(); return x; } } // if we get here it means no similar action was found, we have to create it if (instances < lastaction + 2) { actionObj->initialize(instances, actionObj->getMetaObject()); objManager->registerObject(actionObj); actionObj->setData(actionFields); actionObj->updated(); qDebug() << "ModelUAVProxy:" << "created new action instance:" << instances; return lastaction + 1; } else { PathAction *action = PathAction::GetInstance(objManager, lastaction + 1); Q_ASSERT(action); action->setData(actionFields); action->updated(); actionObj->deleteLater(); qDebug() << "ModelUAVProxy:" << "reused action instance:" << lastaction + 1; return lastaction + 1; } return -1; // error we should never get here }