rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-05 16:46:06 +01:00
Code Issues Releases Activity
ddf2ccf720
LibrePilot/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp
PT_Dreamer a01930b12f GCS-PathPlanner 
Fixes UAVO to DataModel convertion.
2012-10-30 15:13:52 +00:00

250 lines
10 KiB
C++
Raw Blame History

/**
******************************************************************************
*
* @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 <math.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);
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
}
Reference in New Issue View Git Blame Copy Permalink