/// <summary>
/// Calculates the Distance from the Current Position to the given Waypoint in ?? NM ??
/// </summary>
/// <param name="gps">A object representing the onboard GPS</param>
/// <param name="currentWaypoint">A Waypoint representing the Target for the Distance Calc</param>
/// <returns>The Distance in ?? NM?? as a double value</returns>
public double Distance(UAVStructure gps, UAVCommons.Navigation.WayPoint currentWaypoint)
{
if ((gps.values["lbRMCPositionLongitude"] != null) && (gps.values["lbRMCPositionLatitude"] != null))
{
SharpGis.SharpGps.Coordinate gpslocation = new SharpGis.SharpGps.Coordinate(Convert.ToDouble(gps.values["lbRMCPositionLongitude"].Value), Convert.ToDouble(gps.values["lbRMCPositionLatitude"].Value));
double greatCircleDistance = gpslocation.Distance(new SharpGis.SharpGps.Coordinate(currentWaypoint.Longitude, currentWaypoint.Latitude));
double altDistance = Math.Max(Convert.ToDouble(gps.values["lbGGAAltitude"].Value), currentWaypoint.Altitude) - Math.Min(Convert.ToDouble(gps.values["lbGGAAltitude"].Value), currentWaypoint.Altitude);
return(Math.Sqrt(greatCircleDistance * greatCircleDistance + altDistance * altDistance));
}
else
{
// Log error
}
return(99999999);
}
/// <summary>
/// Calculates the Distance from the Current Position to the given Waypoint in ?? NM ??
/// </summary>
/// <param name="gps">A object representing the onboard GPS</param>
/// <param name="currentWaypoint">A Waypoint representing the Target for the Distance Calc</param>
/// <returns>The Distance in ?? NM?? as a double value</returns>
public double Distance(UAVStructure gps, UAVCommons.Navigation.WayPoint currentWaypoint)
{
if ((gps.values["lbRMCPositionLongitude"] != null) && (gps.values["lbRMCPositionLatitude"] != null))
{
SharpGis.SharpGps.Coordinate gpslocation = new SharpGis.SharpGps.Coordinate(Convert.ToDouble(gps.values["lbRMCPositionLongitude"].Value), Convert.ToDouble(gps.values["lbRMCPositionLatitude"].Value));
double greatCircleDistance = gpslocation.Distance(new SharpGis.SharpGps.Coordinate(currentWaypoint.Longitude, currentWaypoint.Latitude));
double altDistance = Math.Max(Convert.ToDouble(gps.values["lbGGAAltitude"].Value), currentWaypoint.Altitude) - Math.Min(Convert.ToDouble(gps.values["lbGGAAltitude"].Value), currentWaypoint.Altitude);
return Math.Sqrt(greatCircleDistance * greatCircleDistance + altDistance * altDistance);
}
else {
// Log error
}
return 99999999;
}
