public int updateNavigation(int waypointRadius)
{
if (!runNavigation)
{
return(-1);
}
// Check: is GPS ready?
if (!mDrone.iDroneCup_isGpsReady())
{
this.runNavigation = false;
mDrone.iDroneCup_Hover();
mDrone.iDroneCup_Land();
System.Diagnostics.Debug.WriteLine("updateNavigation -> GPS lost");
return(-2);
}
// Measure distance to waypoint and home
currCoordinates = new Coordinates(mDrone.iDroneCup_Read_Longitude(),
mDrone.iDroneCup_Read_Latitude());
dist2waypoint = calculateDistance(currCoordinates, nextCoordinates);
dist2home = calculateDistance(currCoordinates, homeCoordinates);
// Apply Navigation Algorithm
if (!busyNavigation)
{
Task <int> .Factory.StartNew(() => doNavigation(waypointRadius));
}
return(0);
}