PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount)
{
int instancesCount = objMngr->getNumInstances(PathAction::OBJID);
int count = actionCount <= instancesCount ? actionCount : instancesCount;
for (int i = 0; i < count; ++i) {
PathAction *action = PathAction::GetInstance(objMngr, i);
Q_ASSERT(action);
if (!action) {
continue;
}
PathAction::DataFields fields = action->getData();
if (fields.Command == actionData.Command
&& fields.ConditionParameters[0] == actionData.ConditionParameters[0]
&& fields.ConditionParameters[1] == actionData.ConditionParameters[1]
&& fields.ConditionParameters[2] == actionData.ConditionParameters[2]
&& fields.EndCondition == actionData.EndCondition
&& fields.ErrorDestination == actionData.ErrorDestination
&& fields.JumpDestination == actionData.JumpDestination && fields.Mode == actionData.Mode
&& fields.ModeParameters[0] == actionData.ModeParameters[0]
&& fields.ModeParameters[1] == actionData.ModeParameters[1]
&& fields.ModeParameters[2] == actionData.ModeParameters[2]) {
return action;
}
}
return NULL;
}
PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction)
{
PathAction *action = NULL;
int count = objMngr->getNumInstances(PathAction::OBJID);
if (index < count) {
// reuse object
qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count;
action = PathAction::GetInstance(objMngr, index);
if (newAction) {
newAction->deleteLater();
}
} else if (index < count + 1) {
// create "next" object
qDebug() << "ModelUAVProxy::createPathAction - created action instance :" << index;
// TODO is there a limit to the number of path actions?
action = newAction ? newAction : new PathAction;
action->initialize(index, action->getMetaObject());
objMngr->registerObject(action);
} else {
// we can only create the "next" object
// TODO fail in a clean way :(
}
return action;
}
bool ModelUavoProxy::objectsToModel()
{
// build model from uav objects
// the list of objects can end with "garbage" instances due to previous flightpath
// they need to be ignored
PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
PathPlan::DataFields pathPlanData = pathPlan->getData();
int waypointCount = pathPlanData.WaypointCount;
int actionCount = pathPlanData.PathActionCount;
// consistency check
if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) {
QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan way point count error !"));
return false;
}
if (actionCount > objMngr->getNumInstances(PathAction::OBJID)) {
QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan path action count error !"));
return false;
}
if (pathPlanData.Crc != computePathPlanCrc(waypointCount, actionCount)) {
QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Path plan CRC error !"));
return false;
}
int rowCount = myModel->rowCount();
if (waypointCount < rowCount) {
myModel->removeRows(waypointCount, rowCount - waypointCount);
} else if (waypointCount > rowCount) {
myModel->insertRows(rowCount, waypointCount - rowCount);
}
for (int i = 0; i < waypointCount; ++i) {
Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
Q_ASSERT(waypoint);
if (!waypoint) {
continue;
}
Waypoint::DataFields waypointData = waypoint->getData();
waypointToModel(i, waypointData);
PathAction *action = PathAction::GetInstance(objMngr, waypoint->getAction());
Q_ASSERT(action);
if (!action) {
continue;
}
PathAction::DataFields actionData = action->getData();
pathActionToModel(i, actionData);
}
return true;
}
quint8 ModelUavoProxy::computePathPlanCrc(int waypointCount, int actionCount)
{
quint8 crc = 0;
for (int i = 0; i < waypointCount; ++i) {
Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
crc = waypoint->updateCRC(crc);
}
for (int i = 0; i < actionCount; ++i) {
PathAction *action = PathAction::GetInstance(objMngr, i);
crc = action->updateCRC(crc);
}
return crc;
}
PathAction* DebugMenu::FindPath(const String& name)
{
for (IAction* action : m_ActionList)
{
if (action->type == IAction::Type::PATH)
{
PathAction* a = (PathAction*)action;
if (a->name == name)
return a;
else
a->FindPath(name);
}
}
return nullptr;
}
void DebugMenu::Add(const String& path, IAction* action)
{
if (StringContains(path, "/"))
{
std::vector<String> paths = SplitString(path, "/");
action->name = paths.back();
paths.pop_back();
PathAction* pathAction = s_Instance->CreateOrFindPaths(paths);
SP_ASSERT(pathAction);
if (!pathAction->ContainsAction(action->name))
pathAction->actionList.push_back(action);
else
spdel action;
}
else
{
s_Instance->m_ActionList.push_back(action);
}
s_Instance->Refresh();
}
void ModelUavoProxy::receivePathPlan()
{
PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0);
connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0);
connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
PathAction *action = PathAction::GetInstance(objMngr, 0);
connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
// we will start 3 update requests
completionCountdown = 3;
successCountdown = completionCountdown;
pathPlan->requestUpdate();
waypoint->requestUpdateAll();
action->requestUpdateAll();
}
void ModelUavoProxy::sendPathPlan()
{
modelToObjects();
PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0);
connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)),
this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0);
connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)),
this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
PathAction *action = PathAction::GetInstance(objMngr, 0);
connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)),
this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
// we will start 3 update all
completionCountdown = 3;
successCountdown = completionCountdown;
pathPlan->updated();
waypoint->updatedAll();
action->updatedAll();
}
void DebugMenu::Remove(const String& path)
{
if (StringContains(path, "/"))
{
std::vector<String> paths = SplitString(path, "/");
String name = paths.back();
paths.pop_back();
PathAction* pathAction = s_Instance->CreateOrFindPaths(paths);
SP_ASSERT(pathAction);
if (pathAction->ContainsAction(name))
{
if (pathAction->actionList.size() == 1)
{
PathAction* parent = pathAction->parent;
if (parent)
{
parent->DeleteChild(pathAction);
}
else
{
for (uint i = 0; i < s_Instance->m_ActionList.size(); i++)
{
if (s_Instance->m_ActionList[i] == pathAction)
{
spdel s_Instance->m_ActionList[i];
s_Instance->m_ActionList.erase(s_Instance->m_ActionList.begin() + i);
break;
}
}
}
while (parent)
{
spdel pathAction;
pathAction = pathAction->parent;
}
}
else
{
ActionList& actionList = pathAction->actionList;
for (uint i = 0; i < actionList.size(); i++)
{
if (actionList[i]->name == name)
{
actionList.erase(actionList.begin() + i);
break;
}
}
}
}
}
else
{
ActionList& actions = s_Instance->m_ActionList;
for (uint i = 0; i < actions.size(); i++)
{
if (actions[i]->name == path)
{
spdel actions[i];
actions.erase(actions.begin() + i);
break;
}
}
}
s_Instance->Refresh();
}
// update waypoint and path actions UAV objects
//
// waypoints are unique and each waypoint has an entry in the UAV waypoint list
//
// a path action can be referenced by multiple waypoints
// waypoints reference path action by their index in the UAV path action list
// the compression of path actions happens here.
// (compression consists in keeping only one instance of similar path actions)
//
// the UAV waypoint list and path action list are probably not empty, so we try to reuse existing instances
bool ModelUavoProxy::modelToObjects()
{
qDebug() << "ModelUAVProxy::modelToObjects";
// track number of path actions
int actionCount = 0;
// iterate over waypoints
int waypointCount = myModel->rowCount();
for (int i = 0; i < waypointCount; ++i) {
// Path Actions
// create action to use as a search criteria
// this object does not need to be deleted as it will either be added to the managed list or deleted later
PathAction *action = new PathAction;
// get model data
PathAction::DataFields actionData = action->getData();
modelToPathAction(i, actionData);
// see if that path action has already been added in this run
PathAction *foundAction = findPathAction(actionData, actionCount);
// TODO this test needs a consistency check as it is unsafe.
// if the find method is buggy and returns false positives then the flight plan sent to the uav is broken!
// the find method should do a "binary" compare instead of a field by field compare
// if a field is added everywhere and not in the compare method then you can start having false positives
if (!foundAction) {
// create or reuse an action instance
action = createPathAction(actionCount, action);
actionCount++;
// update UAVObject
action->setData(actionData);
} else {
action->deleteLater();
action = foundAction;
qDebug() << "ModelUAVProxy::modelToObjects - found action instance :" << action->getInstID();
}
// Waypoints
// create or reuse a waypoint instance
Waypoint *waypoint = createWaypoint(i, NULL);
Q_ASSERT(waypoint);
// get model data
Waypoint::DataFields waypointData = waypoint->getData();
modelToWaypoint(i, waypointData);
// connect waypoint to path action
waypointData.Action = action->getInstID();
// update UAVObject
waypoint->setData(waypointData);
}
// Put "safe" values in unused waypoint and path action objects
if (waypointCount < objMngr->getNumInstances(Waypoint::OBJID)) {
for (int i = waypointCount; i < objMngr->getNumInstances(Waypoint::OBJID); ++i) {
// TODO
}
}
if (actionCount < objMngr->getNumInstances(PathAction::OBJID)) {
for (int i = actionCount; i < objMngr->getNumInstances(PathAction::OBJID); ++i) {
// TODO
}
}
// Update PathPlan
PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
PathPlan::DataFields pathPlanData = pathPlan->getData();
pathPlanData.WaypointCount = waypointCount;
pathPlanData.PathActionCount = actionCount;
pathPlanData.Crc = computePathPlanCrc(waypointCount, actionCount);
pathPlan->setData(pathPlanData);
return true;
}
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+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]);
}
}