/// <summary>
/// Passing old Waypoint select new Waypoint
/// </summary>
/// <param name="waypoint"></param>
void navi_PassingWayPoint(UAVCommons.Navigation.WayPoint waypoint)
{
if (values["WaypointIndex"].IntValue < Waypoints.Count)
{
values["WaypointIndex"].IntValue++;
}
navigation.currentWaypoint = Waypoints[values["WaypointIndex"].IntValue];
}
/// <summary>
/// Returns the Direction from the Current Position to the given Waypoint
/// </summary>
/// <param name="gps">The Current GPS</param>
/// <param name="myWaypoint">A Waypoint Object containing the position of the Target Waypoint</param>
/// <returns>The Bearing from the Current Position to the Waypoint in degrees (0..360)</returns>
public double GetDirection(GPS gps, UAVCommons.Navigation.WayPoint myWaypoint)
{
double lat1 = Convert.ToDouble(gps["lbRMCPositionLatitude"].Value);
double lat2 = myWaypoint.Latitude;
double lon1 = Convert.ToDouble(gps["lbRMCPositionLongitude"].Value);
double lon2 = myWaypoint.Longitude;
// Calculate Bearing in Radians
double resultRad = (Math.Atan2((Math.Cos(lat1) * Math.Sin(lat2)) - (Math.Sin(lat1) * Math.Cos(lat2) * Math.Cos(lon2 - lon1)),
Math.Sin(lon2 - lon1) * Math.Cos(lat2)) % (2 * Math.PI));
return(resultRad * (180 / Math.PI));
} // Convert Radians to Degrees
private GMap.NET.PointLatLng CreateWayPoint(GMap.NET.PointLatLng point)
{
frm = new Navigation.EditWayPoint(1200, true);
if (frm.Altitude == 0)
{
if (frm.ShowDialog() == DialogResult.Abort)
{
return(new GMap.NET.PointLatLng());
}
}
route.Points.Add(point);
UAVCommons.Navigation.WayPoint wpoint = new UAVCommons.Navigation.WayPoint();
wpoint.Longitude = point.Lng;
wpoint.Latitude = point.Lat;
string altstring = " (";
altstring += frm.Altitude;
wpoint.IsAbsolute = frm.IsAbsolute;
if (frm.IsAbsolute)
{
wpoint.Altitude = frm.Altitude;
altstring += "M";
wpoint.AltitudeAGL = 0;
}
else
{
wpoint.AltitudeAGL = frm.Altitude;
wpoint.Altitude = 0;
altstring += "M AGL";
}
altstring += ")";
WayPoints.Add(wpoint);
GMapMarker mymarker = new GMapMarkerGoogleRed(point);
mymarker.Tag = route.Points.Count - 1;
mymarker.ToolTipMode = MarkerTooltipMode.Always;
mymarker.ToolTipText = "Wegpunkt " + Convert.ToString(route.Points.Count) + altstring;
overlay.Markers.Add(mymarker);
route.Overlay.IsVisibile = true;
route.IsVisible = true;
gMapControl1.UpdateRouteLocalPosition(route);
gMapControl1.UpdateMarkerLocalPosition(mymarker);
return(point);
}
/// <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>
/// When Passing Waypoint, report to Ground Control
/// </summary>
/// <param name="waypoint"></param>
void navi_PassingWayPoint(UAVCommons.Navigation.WayPoint waypoint)
{
UAVCommons.Commands.WriteToFlightLog logcmd = new WriteToFlightLog("Passing WayPoint: " + waypoint.Latitude + "," + waypoint.Longitude, DateTime.Now);
this.SendCommandAsync(logcmd);
}
private GMap.NET.PointLatLng CreateWayPoint(GMap.NET.PointLatLng point)
{
frm = new Navigation.EditWayPoint(1200, true);
if (frm.Altitude == 0)
{
if (frm.ShowDialog() == DialogResult.Abort) return new GMap.NET.PointLatLng();
}
route.Points.Add(point);
UAVCommons.Navigation.WayPoint wpoint = new UAVCommons.Navigation.WayPoint();
wpoint.Longitude = point.Lng;
wpoint.Latitude = point.Lat;
string altstring = " (";
altstring += frm.Altitude;
wpoint.IsAbsolute = frm.IsAbsolute;
if (frm.IsAbsolute)
{
wpoint.Altitude = frm.Altitude;
altstring += "M";
wpoint.AltitudeAGL = 0;
}
else
{
wpoint.AltitudeAGL = frm.Altitude;
wpoint.Altitude = 0;
altstring+= "M AGL";
}
altstring += ")";
WayPoints.Add(wpoint);
GMapMarker mymarker = new GMapMarkerGoogleRed(point);
mymarker.Tag = route.Points.Count - 1;
mymarker.ToolTipMode = MarkerTooltipMode.Always;
mymarker.ToolTipText = "Wegpunkt " + Convert.ToString(route.Points.Count) + altstring;
overlay.Markers.Add(mymarker);
route.Overlay.IsVisibile = true;
route.IsVisible = true;
gMapControl1.UpdateRouteLocalPosition(route);
gMapControl1.UpdateMarkerLocalPosition(mymarker);
return point;
}
