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