コード例 #1
0
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;
}
コード例 #2
0
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;
}
コード例 #3
0
// 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;
}
コード例 #4
0
ファイル: modeluavoproxy.cpp プロジェクト: BangBo/OpenPilot
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]);
    }
}