Whenever we wish our drone/U.A.V. to loiter we load a waypoint (x,y,z where z is height) and call the Loiter flag (F_LOITER) along with it. On the remote of our U.A.V. its in the auto mode. The Loiter flag has a program which guides the U.A.V. We are going to bring changes in this program.
The basic idea is that whenever a waypoint is loaded for loiter, 8 more waypoints are calculated around it and loaded one after another. to calculate the waypoints we use Switch case in C language with the following logic:
- We know - Sin of any angle = Perpendicular/Hypotenuse and Cos of any angle = Base/Hypotenuse
- Therefore, Sin45 = 1/1.414 and Cos45 = 1/1.414 (root of 2 = 1.414)
- Hence if we load a waypoint for eg (0,0) with waypoint/loiter radius 1000 meters then our code will look like this -
void loiter_routine()
{
struct relative3D myltrpnt;
loitercount++; //set_goal(GPSlocation, wp_to_relative(currentWaypointSet[waypointIndex]).loc)
switch(loitercount)
{
case 1:
myltrpnt.x= currentWaypointSet[waypointIndex].loc.x;
myltrpnt.y= currentWaypointSet[waypointIndex].loc.y + LOITER_RADIUS;
myltrpnt.z= currentWaypointSet[waypointIndex].loc.z;
break;
case 2:
myltrpnt.x= (currentWaypointSet[waypointIndex].loc.x)-(LOITER_RADIUS/(1.414));
myltrpnt.y= (currentWaypointSet[waypointIndex].loc.y)+(LOITER_RADIUS/(1.414));
myltrpnt.z= currentWaypointSet[waypointIndex].loc.z;
break;
case 3:
myltrpnt.x= currentWaypointSet[waypointIndex].loc.x - LOITER_RADIUS;
myltrpnt.y= (currentWaypointSet[waypointIndex].loc.y);
myltrpnt.z= currentWaypointSet[waypointIndex].loc.z;
break;
case 4:
myltrpnt.x= (currentWaypointSet[waypointIndex].loc.x)-(LOITER_RADIUS/(1.414));
myltrpnt.y= (currentWaypointSet[waypointIndex].loc.y)-(LOITER_RADIUS/(1.414));
myltrpnt.z= currentWaypointSet[waypointIndex].loc.z;
break;
case 5:
myltrpnt.x= (currentWaypointSet[waypointIndex].loc.x);
myltrpnt.y= currentWaypointSet[waypointIndex].loc.y - LOITER_RADIUS;
myltrpnt.z= currentWaypointSet[waypointIndex].loc.z;
break;
case 6:
myltrpnt.x= (currentWaypointSet[waypointIndex].loc.x)+(LOITER_RADIUS/(1.414));
myltrpnt.y= (currentWaypointSet[waypointIndex].loc.y)-(LOITER_RADIUS/(1.414));
myltrpnt.z= currentWaypointSet[waypointIndex].loc.z;
break;
case 7:
myltrpnt.x= (currentWaypointSet[waypointIndex].loc.x)+(LOITER_RADIUS/(1.414));
myltrpnt.y= currentWaypointSet[waypointIndex].loc.y;
myltrpnt.z= currentWaypointSet[waypointIndex].loc.z;
break;
case 8:
myltrpnt.x= (currentWaypointSet[waypointIndex].loc.x)+(LOITER_RADIUS/(1.414));
myltrpnt.y= (currentWaypointSet[waypointIndex].loc.y)+(LOITER_RADIUS/(1.414));
myltrpnt.z= currentWaypointSet[waypointIndex].loc.z;
loitercount =0;
break;
default:
break;
}
set_goal( GPSlocation, myltrpnt ) ;
}