public bool ProximityCheck(double L1, double Lat, double Lon, double Alt, double psi)
{
if (proximityCheck(L1, Lat, Lon, Alt))
{
if (isInitialPath)
{
isInitialPath = false;
currentObjective = 0;
currentPath = auxPath;
ResetArc();
}
else
{
isTurn180 = !isTurn180;
if (isTurn180)
{
currentObjective = 0;
pathGenerator.InitialPosition = currentPath.LastPoint;
pathGenerator.InitialYaw = psi;
pathGenerator.InitialPitch = 0; // just turn, do not change altitude
pathGenerator.InitialEnuPosition = initialEnuPosition;
pathGenerator.newPath();
pathGenerator.TurnRadius = turnRadius;
pathGenerator.Turn180(currentPath.LastPoint, psi);
auxPath = currentPath;
currentPath = pathGenerator.Path;
ResetArc();
}
else
{
currentObjective = 0;
currentPath = auxPath;
ResetArc();
currentPath.Reverse();
}
}
return(true);
}
return(false);
}
public bool VelocityCheck(double s_dot, double dt, double psi)
{
if (currentObjective > landingPoint && landingPoint != 0)
{
isLanding = true;
}
if (velocityCheck(s_dot, dt)) // if true means that it reach the last point.
{
if (isLanding)
{
return(true);
}
if (isInitialPath)
{
isInitialPath = false;
currentPath = auxPath;
if (!isReturnFromCircle)
{
currentObjective = 0;
}
else
{
currentObjective = currentObjectiveOld;
isReturnFromCircle = false;
isCirclePath = false;
}
}
else if (isCirclePath)
{
if (!isReturnFromCircle)
{
currentObjective = 0;
}
else
{
PointLatLngAlt endCircle = currentPath.PointList[currentObjective];
currentObjective = currentObjectiveOld;
currentPath = auxCirclePath;
CreateInitialPath(psi, endCircle.Lat, endCircle.Lng, endCircle.Alt, 215); // FIX ME hardcode velocity
currentObjective = 0;
}
}
else if (isTakeoff)
{
isTakeoff = false;
PointLatLngAlt endTakeoff = currentPath.PointList[currentObjective];
currentObjective = 0;
currentPath = auxPath;
ResetArc();
CreateInitialPath(endTakeoff, psi, 215); // FIX ME hardcode velocity
}
else // End of waypoints turn 180 and continue with the waypoints.
{
isTurn180 = !isTurn180;
if (isTurn180)
{
currentObjective = 0;
pathGenerator.InitialPosition = currentPath.LastPoint; // pointListObjectives[pointListObjectives.Count - 1];
pathGenerator.InitialYaw = psi;
pathGenerator.InitialPitch = 0;
pathGenerator.CurrentVelocity = 215; // FIX ME
pathGenerator.InitialEnuPosition = initialEnuPosition;
pathGenerator.newPath();
pathGenerator.TurnRadius = turnRadius;
pathGenerator.Turn180(currentPath.LastPoint, psi);
auxPath = currentPath;
currentPath = pathGenerator.Path;
ResetArc();
}
else
{
currentObjective = 0;
auxPath = currentPath;
currentPath.Reverse();
ResetArc();
}
}
return(true);
}
return(false);
}