public int doNavigation(int waypointRadius)
{
busyNavigation = true;
float maxDroneSpeedToDestination = 0.5f;
float maxDroneSpeedRotation = 0.01f;
float angleToWaypoint = 0;
currCoordinates = new Coordinates(mDrone.iDroneCup_Read_Longitude(),
mDrone.iDroneCup_Read_Latitude());
// DEGUB //
//currCoordinates.latitude = 41.536955;
//currCoordinates.longitude = -8.627266;
//homeCoordinates = currCoordinates;
// DEGUB //
// Check: is out of range? in wrong direction?
dist2waypoint = calculateDistance(currCoordinates, homeCoordinates);
if (dist2waypoint > SAFE_GPS_RANGE)
{
this.runNavigation = false;
mDrone.iDroneCup_Hover();
mDrone.iDroneCup_Land();
System.Threading.Thread.Sleep(10);
System.Diagnostics.Debug.WriteLine("doNavigation -> Check: is out of range? : " + dist2waypoint.ToString());
}
// Check: is at waypoint?
else if (isAtWaypoint(waypointRadius))
{
// Disable navigation OR do something else: Next waypoint? Action? Land?
this.runNavigation = false;
mDrone.iDroneCup_Hover();
System.Threading.Thread.Sleep(10);
System.Diagnostics.Debug.WriteLine("doNavigation -> Check: is at waypoint?");
}
// Action: Navigation
else
{
// Read Drone Heading
float trueNorthHeading = mDrone.iDroneCup_Read_Heading(0);
droneHeading = trueNorthHeading;
// Check: Rotation Error
float bearing = calculateBearing(currCoordinates, nextCoordinates);
float angleDiff = bearing - trueNorthHeading;
angleToWaypoint = (float)(Math.Atan2(Math.Sin(angleDiff * DEG2RAD), Math.Cos(angleDiff * DEG2RAD)) * RAD2DEG);
droneBearing = bearing;
// Check: Distance Error
float distanceToWaypoint = calculateDistance(currCoordinates, nextCoordinates);
// Compute: Rotation Correction
float yawSpeedDesired = angleToWaypoint * maxDroneSpeedRotation;
yawSpeedDesired = KeepInRange(yawSpeedDesired, -0.25f, 0.25f);
// Compute: Motion Correction
float tmpsin = (float)Math.Sin(angleToWaypoint * DEG2RAD);
float tmpcos = (float)Math.Cos(angleToWaypoint * DEG2RAD);
float pitchSpeedDesired = (maxDroneSpeedToDestination * tmpcos);
float rollSpeedDesired = (maxDroneSpeedToDestination * tmpsin);
rollSpeedDesired = KeepInRange(rollSpeedDesired, -0.25f, 0.25f);
pitchSpeedDesired = KeepInRange(pitchSpeedDesired, -0.25f, 0.25f);
// Optional: Initial yaw correction
if (angleToWaypoint > 40.0f || angleToWaypoint < -40.0f)
{
// Apply only Rotation Correction
//mDrone.iDroneCup_MoveAdvanced(0,0,0,yawSpeedDesired);
}
else
{
// Apply Global Correction
//mDrone.iDroneCup_MoveAdvanced(pitchSpeedDesired,rollSpeedDesired,0,yawSpeedDesired);
}
}
// DEBUG //
float tickDiff = System.Environment.TickCount - lastTick;
lastTick = System.Environment.TickCount;
fps = (int)Math.Round(1000.0f / tickDiff);
System.Diagnostics.Debug.WriteLine("doNavigation -> Navigation done... " + "FPS: " + fps.ToString() + "angle: " + angleToWaypoint.ToString());
// DEBUG //
busyNavigation = false;
return(0);
}