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); }