PathFinder::~PathFinder()
{
//Set the level pointer to NULL.
m_Level = NULL;
//Clear all the path nodes.
clearPathNodes();
}
void PathFinder::findPath(Tile* aCurrentTile, Tile* aDestinationTile)
{
//Safty Check that the sate is Idle, if it isnt tgen a oath s aalready found
if(m_State != StateIdle)
{
return;
}
for(int i = 0; i < m_Level->getNumberOfTiles(); i++)
{
Tile* tile = m_Level->getTileForIndex(i);
if(tile != NULL && tile->isWalkableTile() == true)
{
tile->setIsPath(false);
}
}
clearPathNodes();
int currentTileIndex = m_Level->getTileIndexForTile(aCurrentTile);
m_DestinationTileIndex = m_Level->getTileIndexForTile(aDestinationTile);
//safety check that the destination tile and the current tile indexes are
if(currentTileIndex == m_DestinationTileIndex)
{
return;
}
//Make sure the desitnation tle is walkable
if(aDestinationTile->isWalkableTile() == false)
{
return;
}
//allocate the current tile's path node and add it to the open list
PathNode* pathNode = new PathNode(aCurrentTile);
addPathNodeToOpenList(pathNode);
m_State = StateSearchingPath;
m_SearchDelay = 0.0f;
}
void PathFinder::findPath(Tile* aStartingTile, Tile* aDestinationTile)
{
//Reset all the tile path flags.
for (int i = 0; i < m_Level->getNumberOfTiles(); i++)
{
Tile* tile = m_Level->getTileForIndex(i);
if(tile != NULL && tile->isWalkableTile() == true)
{
tile->setIsPath(false);
}
}
//Next clear all the path nodes.
clearPathNodes();
//Next get the current and estination tiles indexes.
int currentTileIndex = m_Level->getTileIndexForTile(aStartingTile);
m_DestinationTileIndex = m_Level->getTileIndexForTile(aDestinationTile);
//Make sure they are not the same tile
if(currentTileIndex == m_DestinationTileIndex)
{
return;
}
//Make sure the destination tile is a walkable tile
if(aDestinationTile->isWalkableTile() == false)
{
return;
}
//Create a path node for the starting tile and add it to the Open List
PathNode* pathNode = new PathNode(aStartingTile);
addNodeToOpenList(pathNode);
//Lastly set the state to 'searching path'
m_State = StateSearchingPath;
}
PathFinder::~PathFinder()
{
clearPathNodes();
}
void checkAMData(){
if (!newInterchipData()){
return;
}
switch (interchip_receive_buffer.am_data.command){
case PM_DEBUG_TEST:
// UART1_SendString("Test");
break;
case PM_NEW_WAYPOINT:;
PathData* node = initializePathNode();
node->altitude = interchip_receive_buffer.am_data.waypoint.altitude;
node->latitude = interchip_receive_buffer.am_data.waypoint.latitude;
node->longitude = interchip_receive_buffer.am_data.waypoint.longitude;
node->radius = interchip_receive_buffer.am_data.waypoint.radius;
node->type = interchip_receive_buffer.am_data.waypoint.type;
appendPathNode(node);
// debug("new");
// char str[20];
// sprintf(str, "Lat: %f, Lon: %f, count: %d", node->latitude, node->longitude, pathCount);
// debug(str);
break;
case PM_CLEAR_WAYPOINTS:
clearPathNodes();
break;
case PM_INSERT_WAYPOINT:
node = initializePathNode();
node->altitude = interchip_receive_buffer.am_data.waypoint.altitude;
node->latitude = interchip_receive_buffer.am_data.waypoint.latitude;
node->longitude = interchip_receive_buffer.am_data.waypoint.longitude;
node->radius = interchip_receive_buffer.am_data.waypoint.radius;
node->type = interchip_receive_buffer.am_data.waypoint.type;
insertPathNode(node,interchip_receive_buffer.am_data.waypoint.previousId,interchip_receive_buffer.am_data.waypoint.nextId);
break;
case PM_UPDATE_WAYPOINT:
node = initializePathNode();
node->altitude = interchip_receive_buffer.am_data.waypoint.altitude;
node->latitude = interchip_receive_buffer.am_data.waypoint.latitude;
node->longitude = interchip_receive_buffer.am_data.waypoint.longitude;
node->radius = interchip_receive_buffer.am_data.waypoint.radius;
node->type = interchip_receive_buffer.am_data.waypoint.type;
updatePathNode(node,interchip_receive_buffer.am_data.waypoint.id);
break;
case PM_REMOVE_WAYPOINT:
removePathNode(interchip_receive_buffer.am_data.waypoint.id);
break;
case PM_SET_TARGET_WAYPOINT:
// node = initializePathNode();
// node->altitude = gpsData.altitude;
// node->latitude = gpsData.latitude;
// node->longitude = gpsData.longitude;
// node->radius = 1; //Arbitrary value
// node->type = DEFAULT_WAYPOINT;
if (path[getIndexFromID(interchip_receive_buffer.am_data.waypoint.id)] && path[getIndexFromID(interchip_receive_buffer.am_data.waypoint.id)]->previous){
// insertPathNode(node,path[getIndexFromID(amData.waypoint.id)]->previous->id,amData.waypoint.id);
currentIndex = getIndexFromID(interchip_receive_buffer.am_data.waypoint.id);
}
returnHome = 0;
break;
case PM_SET_RETURN_HOME_COORDINATES:
home.altitude = interchip_receive_buffer.am_data.waypoint.altitude;
home.latitude = interchip_receive_buffer.am_data.waypoint.latitude;
home.longitude = interchip_receive_buffer.am_data.waypoint.longitude;
home.radius = 1;
home.id = -1;
home.type = DEFAULT_WAYPOINT;
break;
case PM_RETURN_HOME:
returnHome = 1;
break;
case PM_CANCEL_RETURN_HOME:
returnHome = 0;
break;
case PM_FOLLOW_PATH:
followPath = interchip_receive_buffer.am_data.followPath;
break;
case PM_EXIT_HOLD_ORBIT:
inHold = false;
break;
case PM_CALIBRATE_ALTIMETER:
calibrateAltimeter(interchip_receive_buffer.am_data.calibrationHeight);
break;
case PM_CALIBRATE_AIRSPEED:
calibrateAirspeed();
break;
case PM_SET_PATH_GAIN:
k_gain[PATH] = interchip_receive_buffer.am_data.pathGain;
break;
case PM_SET_ORBIT_GAIN:
k_gain[ORBIT] = interchip_receive_buffer.am_data.orbitGain;
break;
default:
break;
}
}
