static void
WriteWayPointFile(WayPointList &way_points, FILE *fp,
const SETTINGS_COMPUTER &settings_computer)
{
// remove previous home if it exists in this file
for (unsigned i = 0; way_points.verify_index(i); ++i) {
WAYPOINT &way_point = way_points.set(i);
if (way_point.FileNum == globalFileNum) {
if ((way_point.Flags & HOME) == HOME) {
way_point.Flags -= HOME;
}
}
}
for (unsigned i = 0; way_points.verify_index(i); ++i) {
WAYPOINT &way_point = way_points.set(i);
if (way_point.FileNum == globalFileNum) {
// set home flag if it's the home
if ((int)i == settings_computer.HomeWaypoint) {
if ((way_point.Flags & HOME) != HOME) {
way_point.Flags += HOME;
}
}
WriteWayPointFileWayPoint(fp, &way_point);
}
}
}
static bool
FeedWayPointLine(WayPointList &way_points, RasterTerrain &terrain,
const TCHAR *line)
{
if (TempString[0] == '\0' ||
TempString[0] == 0x1a || // dos end of file
_tcsstr(TempString, TEXT("**")) == TempString || // Look For Comment
_tcsstr(TempString, TEXT("*")) == TempString) // Look For SeeYou Comment
/* nothing was parsed, return without error condition */
return true;
WAYPOINT *new_waypoint = way_points.append();
if (new_waypoint == NULL)
return false; // failed to allocate
new_waypoint->Details = NULL;
if (!ParseWayPointString(new_waypoint, TempString, terrain)) {
way_points.pop();
return false;
}
if (!WaypointInTerrainRange(new_waypoint, terrain)) {
way_points.pop();
return true;
}
return true;
}
int
FindNearestWayPoint(const WayPointList &way_points,
MapWindowProjection &map_projection,
const GEOPOINT &loc,
double MaxRange,
bool exhaustive)
{
int NearestIndex = -1;
double NearestDistance, Dist;
NearestDistance = MaxRange;
for (unsigned i = 0; way_points.verify_index(i); ++i) {
if (way_points.get_calc(i).Visible) {
if (map_projection.WaypointInScaleFilter(i)) {
// only look for visible waypoints
// feature added by Samuel Gisiger
Dist = Distance(loc, way_points.get(i).Location);
if(Dist < NearestDistance) {
NearestIndex = i;
NearestDistance = Dist;
}
}
}
}
// JMW allow exhaustive check for when looking up in status dialog
if (exhaustive && (NearestIndex == -1)) {
for (unsigned i = 0; way_points.verify_index(i); ++i) {
Dist = Distance(loc, way_points.get(i).Location);
if(Dist < NearestDistance) {
NearestIndex = i;
NearestDistance = Dist;
}
}
}
if(NearestDistance < MaxRange)
return NearestIndex;
else
return -1;
}
int
FindMatchingWaypoint(const WayPointList &way_points, WAYPOINT *waypoint)
{
// first scan, lookup by name
for (unsigned i = 0; way_points.verify_index(i); ++i) {
if (_tcscmp(waypoint->Name, way_points.get(i).Name)==0) {
return i;
}
}
// second scan, lookup by location
for (unsigned i = 0; way_points.verify_index(i); ++i) {
const WAYPOINT &wpi = way_points.get(i);
if ((fabs(waypoint->Location.Latitude - wpi.Location.Latitude)<1.0e-6)
&& (fabs(waypoint->Location.Longitude - wpi.Location.Longitude)<1.0e-6)) {
return i;
}
}
return -1;
}
void XCSoarInterface::Shutdown(void) {
CreateProgressDialog(gettext(TEXT("Shutdown, please wait...")));
StartHourglassCursor();
StartupStore(TEXT("Entering shutdown...\n"));
StartupLogFreeRamAndStorage();
// turn off all displays
globalRunningEvent.reset();
StartupStore(TEXT("dlgAirspaceWarningDeInit\n"));
dlgAirspaceWarningDeInit();
CreateProgressDialog(gettext(TEXT("Shutdown, saving logs...")));
// stop logger
logger.guiStopLogger(Basic(),true);
CreateProgressDialog(gettext(TEXT("Shutdown, saving profile...")));
// Save settings
Profile::StoreRegistry();
// Stop sound
StartupStore(TEXT("SaveSoundSettings\n"));
Profile::SaveSoundSettings();
#ifndef DISABLEAUDIOVARIO
// VarioSound_EnableSound(false);
// VarioSound_Close();
#endif
// Stop drawing
CreateProgressDialog(gettext(TEXT("Shutdown, please wait...")));
StartupStore(TEXT("CloseDrawingThread\n"));
closeTriggerEvent.trigger();
calculation_thread->join();
StartupStore(TEXT("- calculation thread returned\n"));
instrument_thread->join();
StartupStore(TEXT("- instrument thread returned\n"));
draw_thread->join();
StartupStore(TEXT("- draw thread returned\n"));
delete draw_thread;
// Clear data
CreateProgressDialog(gettext(TEXT("Shutdown, saving task...")));
StartupStore(TEXT("Resume abort task\n"));
task.ResumeAbortTask(SettingsComputer(), -1); // turn off abort if it was on.
StartupStore(TEXT("Save default task\n"));
task.SaveDefaultTask();
StartupStore(TEXT("Clear task data\n"));
task.ClearTask();
StartupStore(TEXT("Close airspace\n"));
CloseAirspace();
StartupStore(TEXT("Close waypoints\n"));
way_points.clear();
CreateProgressDialog(gettext(TEXT("Shutdown, please wait...")));
StartupStore(TEXT("CloseTerrainTopology\n"));
RASP.Close();
terrain.CloseTerrain();
delete topology;
delete marks;
devShutdown();
SaveCalculationsPersist(Basic(),Calculated());
#if (EXPERIMENTAL > 0)
// CalibrationSave();
#endif
#if defined(GNAV) && !defined(PCGNAV)
StartupStore(TEXT("Altair shutdown\n"));
Sleep(2500);
StopHourglassCursor();
InputEvents::eventDLLExecute(TEXT("altairplatform.dll SetShutdown 1"));
while(1) {
Sleep(100); // free time up for processor to perform shutdown
}
#endif
CloseFLARMDetails();
// Kill windows
StartupStore(TEXT("Destroy Info Boxes\n"));
InfoBoxManager::Destroy();
StartupStore(TEXT("Destroy Button Labels\n"));
ButtonLabel::Destroy();
StartupStore(TEXT("Delete Objects\n"));
// Kill graphics objects
DeleteFonts();
DeleteAirspace();
StartupStore(TEXT("Close Progress Dialog\n"));
CloseProgressDialog();
CloseGeoid();
StartupStore(TEXT("Close Windows - main \n"));
main_window.reset();
StartupStore(TEXT("Close Graphics\n"));
MapGfx.Destroy();
#ifdef DEBUG_TRANSLATIONS
StartupStore(TEXT("Writing missing translations\n"));
WriteMissingTranslations();
#endif
StartupLogFreeRamAndStorage();
StartupStore(TEXT("Finished shutdown\n"));
StopHourglassCursor();
}
static void
CloseWayPoints(WayPointList &way_points)
{
way_points.clear();
WaypointOutOfTerrainRangeDontAskAgain = WaypointsOutOfRange;
}
void
SetHome(const WayPointList &way_points, RasterTerrain &terrain,
SETTINGS_COMPUTER &settings,
const bool reset, const bool set_location)
{
StartupStore(TEXT("SetHome\n"));
// check invalid home waypoint or forced reset due to file change
// VENTA3
if (reset || !way_points.verify_index(0) ||
!way_points.verify_index(settings.HomeWaypoint)) {
settings.HomeWaypoint = -1;
}
// VENTA3 -- reset Alternates
if (reset
|| !way_points.verify_index(settings.Alternate1)
|| !way_points.verify_index(settings.Alternate2)) {
settings.Alternate1= -1;
settings.Alternate2= -1;
}
// check invalid task ref waypoint or forced reset due to file change
if (reset || !way_points.verify_index(settings.TeamCodeRefWaypoint)) {
settings.TeamCodeRefWaypoint = -1;
}
if (!way_points.verify_index(settings.HomeWaypoint)) {
// search for home in waypoint list, if we don't have a home
settings.HomeWaypoint = -1;
for (unsigned i = 0; way_points.verify_index(i); ++i)
{
if ((way_points.get(i).Flags & HOME) == HOME)
{
if (settings.HomeWaypoint== -1) {
settings.HomeWaypoint = i;
break; // only search for one
}
}
}
}
// set team code reference waypoint if we don't have one
if (settings.TeamCodeRefWaypoint== -1) {
settings.TeamCodeRefWaypoint =
settings.HomeWaypoint;
}
if (set_location) {
if (way_points.verify_index(settings.HomeWaypoint)) {
// OK, passed all checks now
StartupStore(TEXT("Start at home waypoint\n"));
const WAYPOINT &home = way_points.get(settings.HomeWaypoint);
device_blackboard.SetStartupLocation(home.Location, home.Altitude);
} else {
// no home at all, so set it from center of terrain if available
GEOPOINT loc;
if (terrain.GetTerrainCenter(&loc)) {
StartupStore(TEXT("Start at terrain center\n"));
device_blackboard.SetStartupLocation(loc, 0);
}
}
}
//
// Save the home waypoint number in the resgistry
//
// VENTA3> this is probably useless, since HomeWayPoint &c were currently
// just loaded from registry.
SetToRegistry(szRegistryHomeWaypoint,settings.HomeWaypoint);
SetToRegistry(szRegistryAlternate1,settings.Alternate1);
SetToRegistry(szRegistryAlternate2,settings.Alternate2);
SetToRegistry(szRegistryTeamcodeRefWaypoint,settings.TeamCodeRefWaypoint);
}
void PathPlanner::slotComputePath(const QVector3D& vehiclePosition, const WayPointList& wayPointList)
{
Q_ASSERT(!wayPointList.isEmpty());
if(!mIsInitialized) initialize();
WayPointList wayPointsWithHighInformationGain = wayPointList;
wayPointsWithHighInformationGain.prepend(WayPoint(vehiclePosition, 0));
qDebug() << __PRETTY_FUNCTION__ << "computing path for waypointlist" << wayPointsWithHighInformationGain.toString();
// mDeviceOccupancyGrid points to device memory filled with grid-values of quint8.
// After fillOccupancyGrid(), empty cells contain a 0, occupied cells contain a 254/255.
// The cell containing the start is set to 1. Then, one thread is launched per cell,
// looking into the neighboring cells. If a neighboring cell (26 3d-neighbors) contains
// a value other than 0 or 254/255, the current cell is set to min(neighbor)+1.
// This is executed often, so that all cells reachable from start get filled with the
// distance TO start
alignPathPlannerGridToColliderCloud();
QTime t;
t.start();
WayPointList computedPath;
copyParametersToGpu(&mParametersPathPlanner);
const bool haveToMapGridOccupancyTemplate = checkAndMapGridOccupancy(mCudaVboResourceGridOccupancyTemplate);
const bool haveToMapGridOccupancyPathPlanner = checkAndMapGridOccupancy(mCudaVboResourceGridPathPlanner);
// Only re-creates the occupancy grid if the collider cloud's content changed
populateOccupancyGrid();
// The first path leads from vehicle position to first waypoint. Clear the occupancy grid above the vehicle!
// This is currently necessary, because we also scan bernd and the fishing rod, occupying our own cells.
clearOccupancyGridAboveVehiclePosition(
mGridOccupancyPathPanner,
vehiclePosition.x(),
vehiclePosition.y(),
vehiclePosition.z(),
&mCudaStream);
// We have freed the occupancy grid a little to make path planning easier/possible. Restore it to the real grid asap.
mRepopulateOccupanccyGrid = true;
// Make some room for waypoints in host memory. The first float4's x=y=z=w will store just the number of waypoints
float* waypointsHost = new float[mMaxWaypoints * 4];
// Find a path between every pair of waypoints
quint32 indexWayPointStart = 0;
quint32 indexWayPointGoal = 1;
quint32 pathNumber = 0;
do
{
mParametersPathPlanner.start = CudaHelper::convert(wayPointsWithHighInformationGain.at(indexWayPointStart));
mParametersPathPlanner.goal = CudaHelper::convert(wayPointsWithHighInformationGain.at(indexWayPointGoal));
qDebug() << __PRETTY_FUNCTION__ << "now computing path from" << indexWayPointStart << ":" << wayPointsWithHighInformationGain.at(indexWayPointStart).toString() << "to" << indexWayPointGoal << ":" << wayPointsWithHighInformationGain.at(indexWayPointGoal).toString();
copyParametersToGpu(&mParametersPathPlanner);
// Copy the populated and dilated occupancy grid into the PathFinder's domain
cudaSafeCall(cudaMemcpy(
mGridOccupancyPathPanner,
mGridOccupancyTemplate,
mParametersPathPlanner.grid.getCellCount(),
cudaMemcpyDeviceToDevice));
// Now start path planning.
markStartCell(mGridOccupancyPathPanner, &mCudaStream);
growGrid(
mGridOccupancyPathPanner,
&mParametersPathPlanner,
&mCudaStream);
// We must set the waypoints-array on the device to a special value because
// a bug can lead to some waypoints not being written. This is ok
// as long as we can detect this. When memsetting with 0, we can,
// otherwise we'd find a waypoint from a previous run and couldn't
// detect this incidence.
cudaSafeCall(cudaMemset(
(void*)mDeviceWaypoints,
255, // interpreted as float, should be NaN!
4 * mMaxWaypoints * sizeof(float)));
retrievePath(
mGridOccupancyPathPanner,
mDeviceWaypoints,
&mCudaStream);
cudaSafeCall(cudaMemcpy(
(void*)waypointsHost,
(void*)mDeviceWaypoints,
4 * mMaxWaypoints * sizeof(float),
cudaMemcpyDeviceToHost));
if(fabs(waypointsHost[0]) < 0.001 && fabs(waypointsHost[1]) < 0.001 && fabs(waypointsHost[2]) < 0.001)
{
qDebug() << __PRETTY_FUNCTION__ << "found NO path from" << indexWayPointStart << ":" << wayPointsWithHighInformationGain.at(indexWayPointStart).toString() << "to" << indexWayPointGoal << ":" << wayPointsWithHighInformationGain.at(indexWayPointGoal).toString();
// When no path was found, we try to find a path to the next waypoint, skipping the problematic one.
indexWayPointGoal++;
}
else
{
qDebug() << __PRETTY_FUNCTION__ << "found path with" << waypointsHost[0] << "waypoints";
// The first waypoint isn't one, it only contains the number of waypoints
for(int i=1; i<=waypointsHost[0]; i++)
{
// workaround for the no-waypoint-bug, which will show values of NaN/NaN/NaN/NaN due to the 255-memset above
if(isnan(waypointsHost[4*i+3]))
{
qDebug() << __PRETTY_FUNCTION__ << "ignoring waypoint that was skpped in retrievePath due to bug in growGrid.";
continue;
}
WayPoint newWayPoint;
if(i == 1)
{
newWayPoint = wayPointsWithHighInformationGain.at(indexWayPointStart);
}
else if(i == (int)waypointsHost[0])
{
newWayPoint = wayPointsWithHighInformationGain.at(indexWayPointGoal);
}
else
{
newWayPoint = WayPoint(QVector3D(waypointsHost[4*i+0], waypointsHost[4*i+1], waypointsHost[4*i+2]), 0);
}
// Append all points of the first path, and then only starting at the second point of the following paths.
// Otherwise, we have the end of path A and the beginning of path B in the list, although they're the same.
if(pathNumber == 0 || i > 1)
{
computedPath.append(newWayPoint);
}
}
qDebug() << __PRETTY_FUNCTION__ << "found path between" << wayPointsWithHighInformationGain.at(indexWayPointStart).toString() << "and" << wayPointsWithHighInformationGain.at(indexWayPointGoal).toString() << ":" << computedPath.toString();
pathNumber++;
indexWayPointStart = indexWayPointGoal;
indexWayPointGoal++;
}
} while(indexWayPointGoal < wayPointsWithHighInformationGain.size());
delete waypointsHost;
if(haveToMapGridOccupancyTemplate) checkAndUnmapGridOccupancy(mCudaVboResourceGridOccupancyTemplate);
// cudaGraphicsUnmapResources(1, &mCudaVboResourceGridPathFinder, 0);
if(haveToMapGridOccupancyPathPlanner) checkAndUnmapGridOccupancy(mCudaVboResourceGridPathPlanner);
// cudaGraphicsUnmapResources(1, &mCudaVboResourceGridOccupancy, 0);
emit path(computedPath.list(), WayPointListSource::WayPointListSourceFlightPlanner);
qDebug() << __PRETTY_FUNCTION__ << "took" << t.elapsed() << "ms.";
}
