// Retrieve the best edge to follow in the cycle joining phase
int Graph::getNextEdge(int edgeIndex, int toNode)
{
updateCycle(edgeIndex, toNode);
std::vector<int> connections = fetchConnections(toNode, edgeIndex);
return selectEdge(connections);
}
void Drone::runUpdateLoop()
{
int runTime = 0;
int updateInterval = 1000/40;
boost::timer::cpu_timer timer;
while(!_stop_flag)
{
timer.start();
// TODO: Think this through really well, find memory leaks!
// Check connection status and handle unexpected loss of connection
if(!_connected.load()) {
notifyConnectionLost();
connectionLost();
return;
}
// Call any miscellaneous functionality needed by implementation
beforeUpdate();
// Process command queue
_commandmutex.lock();
if(_commandqueue.empty())
{
_connected.store(processNoCommand());
if(!_connected.load())
{
continue;
}
}
else
{
for(drone::command command : _commandqueue)
{
_connected.store(processCommand(command));
if(!_connected.load())
{
_commandqueue.clear();
continue;
}
}
}
_commandqueue.clear();
_commandmutex.unlock();
// Retrieve and process navdata
std::shared_ptr<drone::navdata> navdata;
bool newNavdata = decodeNavdata(navdata);
if(newNavdata)
{
notifyNavdataListeners(navdata);
}
// Call any miscellaneous functionality needed by implementation
updateCycle();
// Run at continuous update rate
runTime = timer.elapsed().wall / 1000000;
timer.stop();
if(updateInterval - runTime > 0)
{
boost::this_thread::sleep_for(boost::chrono::milliseconds(updateInterval - runTime));
}
}
}
