mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
GCS/OPMap-Model/UAVO proxy class.
Handles the convertion between flight plan data from the model to/from UAVObjects.
This commit is contained in:
PT_Dreamer
parent
2b85f1a9fd
commit
b84af5c338
@ -1,6 +1,6 @@
|
||||
#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();
|
||||
@ -11,6 +11,26 @@ modelUavoProxy::modelUavoProxy(QObject *parent,flightDataModel * model):QObject(
|
||||
Q_ASSERT(waypointObj != NULL);
|
||||
pathactionObj=PathAction::GetInstance(objManager);
|
||||
Q_ASSERT(pathactionObj != NULL);
|
||||
|
||||
/*
|
||||
int instances=objManager->getNumInstances(pathactionObj->getObjID());
|
||||
PathAction * actionObj=new PathAction;
|
||||
actionObj->initialize(instances,actionObj->getMetaObject());
|
||||
objManager->registerObject(actionObj);
|
||||
actionObj->updated();
|
||||
|
||||
instances=objManager->getNumInstances(pathactionObj->getObjID());
|
||||
actionObj=new PathAction;
|
||||
actionObj->initialize(instances,actionObj->getMetaObject());
|
||||
objManager->registerObject(actionObj);
|
||||
actionObj->updated();
|
||||
|
||||
instances=objManager->getNumInstances(pathactionObj->getObjID());
|
||||
actionObj=new PathAction;
|
||||
actionObj->initialize(instances,actionObj->getMetaObject());
|
||||
objManager->registerObject(actionObj);
|
||||
actionObj->updated();
|
||||
*/
|
||||
}
|
||||
/*WPDESCRITPTION,LATPOSITION,LNGPOSITION,DISRELATIVE,BEARELATIVE,ISRELATIVE,ALTITUDE,
|
||||
VELOCITY,MODE,MODE_PARAMS0,MODE_PARAMS1,MODE_PARAMS2,MODE_PARAMS3,
|
||||
@ -19,69 +39,209 @@ modelUavoProxy::modelUavoProxy(QObject *parent,flightDataModel * model):QObject(
|
||||
*/
|
||||
void modelUavoProxy::modelToObjects()
|
||||
{
|
||||
PathAction * act;
|
||||
act=new PathAction;
|
||||
Waypoint * wp;
|
||||
wp=new Waypoint;
|
||||
Q_ASSERT(act);
|
||||
Q_ASSERT(wp);
|
||||
Waypoint::DataFields waypoint = wp->getData();
|
||||
PathAction::DataFields action = act->getData();
|
||||
PathAction * act=NULL;
|
||||
Waypoint * wp=NULL;
|
||||
QModelIndex index;
|
||||
double distance;
|
||||
double bearing;
|
||||
double altitude;
|
||||
float velocity;
|
||||
int mode;
|
||||
int mode_param[4];
|
||||
int condition;
|
||||
int cond_param[4];
|
||||
int command;
|
||||
int jump;
|
||||
int error;
|
||||
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);
|
||||
velocity=myModel->data(index).toFloat();
|
||||
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);
|
||||
mode=myModel->data(index).toInt();
|
||||
action.Mode=myModel->data(index).toInt();
|
||||
index=myModel->index(x,flightDataModel::MODE_PARAMS0);
|
||||
mode_param[0]=myModel->data(index).toFloat();
|
||||
action.ModeParameters[0]=myModel->data(index).toFloat();
|
||||
index=myModel->index(x,flightDataModel::MODE_PARAMS1);
|
||||
mode_param[1]=myModel->data(index).toFloat();
|
||||
action.ModeParameters[1]=myModel->data(index).toFloat();
|
||||
index=myModel->index(x,flightDataModel::MODE_PARAMS2);
|
||||
mode_param[2]=myModel->data(index).toFloat();
|
||||
action.ModeParameters[2]=myModel->data(index).toFloat();
|
||||
index=myModel->index(x,flightDataModel::MODE_PARAMS3);
|
||||
mode_param[3]=myModel->data(index).toFloat();
|
||||
action.ModeParameters[3]=myModel->data(index).toFloat();
|
||||
|
||||
index=myModel->index(x,flightDataModel::CONDITION);
|
||||
condition=myModel->data(index).toInt();
|
||||
action.EndCondition=myModel->data(index).toInt();
|
||||
index=myModel->index(x,flightDataModel::CONDITION_PARAMS0);
|
||||
cond_param[0]=myModel->data(index).toFloat();
|
||||
action.ConditionParameters[0]=myModel->data(index).toFloat();
|
||||
index=myModel->index(x,flightDataModel::CONDITION_PARAMS1);
|
||||
cond_param[1]=myModel->data(index).toFloat();
|
||||
action.ConditionParameters[1]=myModel->data(index).toFloat();
|
||||
index=myModel->index(x,flightDataModel::CONDITION_PARAMS2);
|
||||
cond_param[2]=myModel->data(index).toFloat();
|
||||
action.ConditionParameters[2]=myModel->data(index).toFloat();
|
||||
index=myModel->index(x,flightDataModel::CONDITION_PARAMS3);
|
||||
cond_param[3]=myModel->data(index).toFloat();
|
||||
action.ConditionParameters[3]=myModel->data(index).toFloat();
|
||||
|
||||
index=myModel->index(x,flightDataModel::COMMAND);
|
||||
command=myModel->data(index).toInt();
|
||||
action.Command=myModel->data(index).toInt();
|
||||
index=myModel->index(x,flightDataModel::JUMPDESTINATION);
|
||||
jump=myModel->data(index).toInt();
|
||||
action.JumpDestination=myModel->data(index).toInt();
|
||||
index=myModel->index(x,flightDataModel::ERRORDESTINATION);
|
||||
error=myModel->data(index).toInt();
|
||||
|
||||
action.ErrorDestination=myModel->data(index).toInt();
|
||||
|
||||
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=acos(wpfields.Position[Waypoint::POSITION_NORTH]/wpfields.Position[Waypoint::POSITION_EAST])*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);
|
||||
|
||||
index=myModel->index(x,flightDataModel::JUMPDESTINATION);
|
||||
myModel->setData(index,actionfields.JumpDestination);
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
@ -11,6 +11,7 @@ class modelUavoProxy:public QObject
|
||||
Q_OBJECT
|
||||
public:
|
||||
explicit modelUavoProxy(QObject *parent, flightDataModel *model);
|
||||
int addAction(PathAction *actionObj, PathAction::DataFields actionFields,int lastaction);
|
||||
public slots:
|
||||
void modelToObjects();
|
||||
void objectsToModel();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user