rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

More work on deleting waypoints

Browse Source
This commit is contained in:
James Cotton 2012-06-02 15:37:44 -05:00
parent 4ad23864c8
commit 39657af8db
2 changed files with 57 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp
View File

@ -128,8 +128,38 @@ void PathCompiler::doDelWaypoint(int index)
// to shift them all by one and set the new "last" waypoint
// to a stop action
// Not implemented yet
Q_ASSERT(false);
UAVObjectManager *objManager = getObjectManager();
Waypoint *waypoint = Waypoint::GetInstance(objManager);
Q_ASSERT(waypoint);
if (!waypoint)
return;
int numWaypoints = objManager->getNumInstances(waypoint->getObjID());
for (int i = index; i < numWaypoints - 1; i++) {
Waypoint *waypointDest = Waypoint::GetInstance(objManager, i);
Q_ASSERT(waypointDest);
Waypoint *waypointSrc = Waypoint::GetInstance(objManager, i + 1);
Q_ASSERT(waypointSrc);
if (!waypointDest || !waypointSrc)
return;
// Copy the data down an index
waypointDest->setData(waypointSrc->getData());
}
// Set the second to last waypoint to stop (and last for safety)
// the functional equivalent to deleting
for (int i = numWaypoints - 2; i < numWaypoints; i++) {
waypoint = Waypoint::GetInstance(objManager, i);
Q_ASSERT(waypoint);
if (waypoint) {
Waypoint::DataFields waypointData = waypoint->getData();
waypointData.Action = Waypoint::ACTION_STOP;
waypoint->setData(waypointData);
}
}
}
/**
@ -137,7 +167,21 @@ void PathCompiler::doDelWaypoint(int index)
*/
void PathCompiler::doDelAllWaypoints()
{
Waypoint *waypointObj = Waypoint::GetInstance(getObjectManager(), 0);
Q_ASSERT(waypointObj);
if (waypointObj == NULL)
return;
int numWaypoints = getObjectManager()->getNumInstances(waypointObj->getObjID());
for (int i = 0; i < numWaypoints; i++) {
Waypoint *waypoint = Waypoint::GetInstance(getObjectManager(), i);
Q_ASSERT(waypoint);
if (waypoint) {
Waypoint::DataFields waypointData = waypoint->getData();
waypointData.Action = Waypoint::ACTION_STOP;
waypoint->setData(waypointData);
}
}
}
/**
@ -159,13 +203,16 @@ void PathCompiler::doUpdateFromUAV()
QList <struct PathCompiler::waypoint> waypoints;
waypoints.clear();
int numWaypoints = objManager->getNumInstances(waypointObj->getObjID());
for (int i = 0; i < numWaypoints; i++) {
bool stopped = false;
for (int i = 0; i < numWaypoints && !stopped; i++) {
Waypoint *waypoint = Waypoint::GetInstance(objManager, i);
Q_ASSERT(waypoint);
if(waypoint == NULL)
return;
waypoints.append(UavoToInternal(waypoint->getData()));
Waypoint::DataFields waypointData = waypoint->getData();
waypoints.append(UavoToInternal(waypointData));
stopped = waypointData.Action == Waypoint::ACTION_STOP;
}
/* Inform visualization */
@ -215,7 +262,6 @@ Waypoint::DataFields PathCompiler::InternalToUavo(struct waypoint internal)
double homeLLA[3];
double LLA[3];
double NED[3];
struct PathCompiler::waypoint internalWaypoint;
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
@ -237,7 +283,7 @@ Waypoint::DataFields PathCompiler::InternalToUavo(struct waypoint internal)
uavo.Position[Waypoint::POSITION_EAST] = NED[1];
uavo.Position[Waypoint::POSITION_DOWN] = NED[2];
uavo.Action = Waypoint::ACTION_NEXT;
uavo.Action = Waypoint::ACTION_PATHTONEXT;
uavo.Velocity[Waypoint::VELOCITY_NORTH] = 5;
uavo.Velocity[Waypoint::VELOCITY_EAST] = 0;

5
ground/openpilotgcs/src/plugins/opmap/pathcompiler.h
View File

@ -103,6 +103,11 @@ public slots:
*/
void doAddWaypoint(struct PathCompiler::waypoint, int position = -1);
/**
* Update waypoint
*/
//void doUpdateWaypoints(struct PathCompiler::waypoint, int position);
/**
* Delete a waypoint
* @param index which waypoint to delete