public async Task SetTrack(PointLatLngAlt myposition, PointLatLngAlt target)
{
var bearing = myposition.GetBearing(target);
var distance = myposition.GetDistance(target);
// prevent div/0 and also point it flat
if (distance == 0)
{
distance = 1000;
}
var curve = getCurve(myposition.Alt, distance / 1000.0);
if (curve > 0)
{
Console.Write("Target below horizon");
}
var heightdelta = target.Alt - myposition.Alt;
// soh cah toa
// in degrees FOV
var fov = Math.Atan(40 / distance) * (180 / Math.PI);
// in zoom X
var z = (float)MathHelper.mapConstrained(fov, FOVMax, FOVMin, ZoomMin, ZoomMax);
await SetRPYAsync(0, Math.Atan(heightdelta / distance) *rad2deg, bearing, z);
}
public List <PointLatLngAlt> calcspline(PointLatLngAlt currentpos, PointLatLngAlt p1, PointLatLngAlt p2)
{
List <PointLatLngAlt> answer = new List <PointLatLngAlt>();
spline_t = spline_t_sq = 0.0f;
spline_p0 = currentpos;
spline_p1 = p1;
spline_p2 = p2;
spline_p0_prime = new Vector3();
double yaw = (currentpos.GetBearing(p1) + 360) % 360;
spline_p0_prime.x = .000001 * Math.Sin(yaw * deg2rad);
spline_p0_prime.y = .000001 * Math.Cos(yaw * deg2rad);
spline_p0_prime.z = spline_p1.z - spline_p0.z;
initialize_spline_segment();
int steps = 30;
foreach (var step in range(steps + 1))
{
spline_t = (1f / steps) * step;
spline_t_sq = spline_t * spline_t;
evaluate_spline();
answer.Add(new PointLatLngAlt(spline_target.x, spline_target.y, spline_target.z, ""));
}
return(answer);
}
public void CreateSurvey()
{
radius = (int)(radius / CurrentState.multiplierdist);
double startangle = 0; // TODO: Calculate angle
PointLatLngAlt center = new PointLatLngAlt(Lat, Lng, alt);
startangle = center.GetBearing(Host.cs.PlannedHomeLocation);
double a = startangle;
double step = 360.0f / points;
if (direction == -1)
{
a += 360;
step *= -1;
}
for (; a <= (startangle + 360) && a >= 0; a += step)
{
float d = radius;
float R = 6371000;
var lat2 = Math.Asin(Math.Sin(Lat * MathHelper.deg2rad) * Math.Cos(d / R) +
Math.Cos(Lat * MathHelper.deg2rad) * Math.Sin(d / R) * Math.Cos(a * MathHelper.deg2rad));
var lon2 = Lng * MathHelper.deg2rad +
Math.Atan2(Math.Sin(a * MathHelper.deg2rad) * Math.Sin(d / R) * Math.Cos(Lat * MathHelper.deg2rad),
Math.Cos(d / R) - Math.Sin(Lat * MathHelper.deg2rad) * Math.Sin(lat2));
PointLatLng pll = new PointLatLng(lat2 * MathHelper.rad2deg, lon2 * MathHelper.rad2deg);
Host.AddWPtoList(MAVLink.MAV_CMD.WAYPOINT, delay, 0, 0, 0, pll.Lng, pll.Lat, alt);
}
Host.InsertWP(0, MAVLink.MAV_CMD.DO_SET_ROI, 0, 0, 0, 0, Lng, Lat, alt);
Host.InsertWP(1, MAVLink.MAV_CMD.DO_CHANGE_SPEED, 0, speed, 0, 0, 0, 0, 0);
}
public static List <PointLatLngAlt> GeneratePath(MAVState MAV)
{
List <PointLatLngAlt> result = new List <PointLatLngAlt>();
MAVLink.mavlink_mission_item_int_t?prevwp = null;
int a = -1;
foreach (var wp in MAV.wps.Values)
{
a++;
if (!prevwp.HasValue)
{
// change firstwp/aka home to valid alt
prevwp = new MAVLink.mavlink_mission_item_int_t?(new MAVLink.mavlink_mission_item_int_t()
{
x = wp.x, y = wp.y, z = 0
});
continue;
}
if (wp.command != (ushort)MAVLink.MAV_CMD.WAYPOINT && wp.command != (ushort)MAVLink.MAV_CMD.SPLINE_WAYPOINT)
{
continue;
}
var wp1 = new PointLatLngAlt(prevwp.Value);
var wp2 = new PointLatLngAlt(wp);
var bearing = wp1.GetBearing(wp2);
var distwps = wp1.GetDistance(wp2);
var startalt = wp1.Alt;
var endalt = wp2.Alt;
if (startalt == 0)
{
startalt = endalt;
}
if (distwps > 5000)
{
continue;
}
for (double d = 0.1; d < distwps; d += 0.1)
{
var pnt = wp1.newpos(bearing, d);
pnt.Alt = startalt + (d / distwps) * (endalt - startalt);
result.Add(pnt);
}
prevwp = wp;
}
return(result);
}
public void SetTrack(PointLatLngAlt myposition, PointLatLngAlt target)
{
var bearing = myposition.GetBearing(target);
var distance = myposition.GetDistance(target);
var curve = getCurve(myposition.Alt, distance);
if (curve > 0)
{
Console.Write("Target below horizon");
}
var heightdelta = target.Alt - myposition.Alt;
SetRPYAsync(0, Math.Tan(heightdelta / distance) * rad2deg, bearing);
}
static PointLatLngAlt calcIntersection(PointLatLngAlt plla, PointLatLngAlt dest, int step = 100)
{
int distout = 10;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
var dist = plla.GetDistance(dest);
var Y = plla.GetBearing(dest);
// 20 km
while (distout < (dist + 100))
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt newposdist = plla.newpos(Y, distout);
newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;
// get another point 'step' infront
PointLatLngAlt newposdist2 = plla.newpos(Y, distout + step);
newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;
// x is dist from plane, y is alt
var newpoint = FindLineIntersection(new PointF(0, (float)plla.Alt),
new PointF((float)dist, (float)dest.Alt),
new PointF((float)distout, (float)newposdist.Alt),
new PointF((float)distout + step, (float)newposdist2.Alt));
if (newpoint.X != 0)
{
newpos = plla.newpos(Y, newpoint.X);
newpos.Alt = newpoint.Y;
return(newpos);
}
distout += step;
}
//addtomap(newpos, distout.ToString());
return(plla);
}
public void UpdatePositions()
{
// add new point to trail
trail.Add(new PointLatLngAlt(GroundMasterDrone.MavState.cs.lat, GroundMasterDrone.MavState.cs.lng, GroundMasterDrone.MavState.cs.alt, ""));
while (trail.Count > 1000)
{
trail.RemoveAt(0);
}
// get current positions and velocitys
foreach (var drone in Drones)
{
if (drone.Location == null)
{
drone.Location = new PointLatLngAlt();
}
drone.Location.Lat = drone.MavState.cs.lat;
drone.Location.Lng = drone.MavState.cs.lng;
drone.Location.Alt = drone.MavState.cs.alt;
if (drone.Velocity == null)
{
drone.Velocity = new Vector3();
}
drone.Velocity.x = Math.Cos(drone.MavState.cs.groundcourse * deg2rad) * drone.MavState.cs.groundspeed;
drone.Velocity.y = Math.Sin(drone.MavState.cs.groundcourse * deg2rad) * drone.MavState.cs.groundspeed;
drone.Velocity.z = drone.MavState.cs.verticalspeed;
drone.TargetVelocity = GroundMasterDrone.Velocity;
}
var targetbearing = GroundMasterDrone.Heading;
//
if (GroundMasterDrone.MavState.cs.wp_dist < Seperation * 1.5)
{
var headingtowp = (int)GroundMasterDrone.MavState.cs.wpno;
var nextwp = headingtowp + 1;
try
{
PointLatLngAlt targetwp = new PointLatLngAlt(GroundMasterDrone.MavState.wps[headingtowp]);
//PointLatLngAlt targetwp = GroundMasterDrone.Location;
PointLatLngAlt nexttargetwp = new PointLatLngAlt(GroundMasterDrone.MavState.wps[nextwp]);
var bearing = targetwp.GetBearing(nexttargetwp);
// point on the wp line for target
var targetpnt = targetwp.newpos(bearing, Seperation);
targetbearing = GroundMasterDrone.Location.GetBearing(targetpnt);
if (Math.Abs(targetbearing - bearing) > 20)
{
//targetbearing = bearing;
}
AirMasterDrone.TargetVelocity.x = Math.Cos(targetbearing * deg2rad) *
GroundMasterDrone.MavState.cs.groundspeed;
AirMasterDrone.TargetVelocity.y = Math.Sin(targetbearing * deg2rad) *
GroundMasterDrone.MavState.cs.groundspeed;
}
catch
{
}
}
else
{
}
// calc airmaster position
AirMasterDrone.TargetLocation = GroundMasterDrone.Location.newpos(targetbearing, Seperation);
AirMasterDrone.TargetLocation.Alt = Altitude;
// send new position to airmaster
AirMasterDrone.SendPositionVelocity(AirMasterDrone.TargetLocation, AirMasterDrone.TargetVelocity * 0.6);
AirMasterDrone.MavState.GuidedMode.x = (float)AirMasterDrone.TargetLocation.Lat;
AirMasterDrone.MavState.GuidedMode.y = (float)AirMasterDrone.TargetLocation.Lng;
AirMasterDrone.MavState.GuidedMode.z = (float)AirMasterDrone.TargetLocation.Alt;
// get the path
List <PointLatLngAlt> newpositions = PlanMove();
List <PointLatLngAlt> newlist = new List <PointLatLngAlt>();
newlist.Add(GroundMasterDrone.Location);
newlist.AddRange(newpositions);
newpositions = newlist;
int a = 0;
// send position and velocity
foreach (var drone in Drones)
{
if (drone.MavState == airmaster)
{
continue;
}
if (drone.MavState == groundmaster)
{
continue;
}
if (a > (newpositions.Count - 1))
{
break;
}
newpositions[a].Alt += Altitude;
// spline control
drone.SendPositionVelocity(newpositions[a], drone.TargetVelocity / 2);
drone.MavState.GuidedMode.x = (float)newpositions[a].Lat;
drone.MavState.GuidedMode.y = (float)newpositions[a].Lng;
drone.MavState.GuidedMode.z = (float)newpositions[a].Alt;
// vel only
//drone.SendVelocity(drone.TargetVelocity);
a++;
}
}
private void map_MouseMove(object sender, MouseEventArgs e)
{
var mousecurrent = map.FromLocalToLatLng(e.X, e.Y);
if (mousedown)
{
mousedragging = true;
if (currentmode == mode.panmode)
{
if (e.Button == MouseButtons.Left)
{
double latdif = mousestart.Lat - mousecurrent.Lat;
double lngdif = mousestart.Lng - mousecurrent.Lng;
try
{
map.Position = new PointLatLng(map.Position.Lat + latdif, map.Position.Lng + lngdif);
}
catch { }
}
}
else if (currentmode == mode.drawbox)
{
if (e.Button == MouseButtons.Left)
{
var rect = RectangleF.FromLTRB((float)mousestart.Lng, (float)mousestart.Lat, (float)mousecurrent.Lng, (float)mousecurrent.Lat);
list.Clear();
// tl
list.Add(mousestart);
// tr
list.Add(new PointLatLng(rect.Top, rect.Right));
// br
list.Add(mousecurrent);
// bl
list.Add(new PointLatLng(rect.Bottom, rect.Left));
if (boxpoly != null)
{
layerpolygons.Polygons.Remove(boxpoly);
}
boxpoly = null;
boxpoly = new GMapPolygon(list, "boxpoly");
boxpoly.IsHitTestVisible = true;
boxpoly.Stroke = new Pen(Color.Red, 2);
boxpoly.Fill = Brushes.Transparent;
layerpolygons.Polygons.Add(boxpoly);
map.Invalidate();
}
}
else if (currentmode == mode.movebox)
{
//if (mouseinsidepoly)
{
double latdif = mousestart.Lat - mousecurrent.Lat;
double lngdif = mousestart.Lng - mousecurrent.Lng;
for (int a = 0; a < boxpoly.Points.Count; a++)
{
boxpoly.Points[a] = new PointLatLng(boxpoly.Points[a].Lat - latdif, boxpoly.Points[a].Lng - lngdif);
}
UpdateListFromBox();
map.UpdatePolygonLocalPosition(boxpoly);
map.Invalidate();
mousestart = mousecurrent;
}
}
else if (currentmode == mode.editbox)
{
double latdif = mousestart.Lat - mousecurrent.Lat;
double lngdif = mousestart.Lng - mousecurrent.Lng;
// 2 point the create the lowest crosstrack distance
// extend at 90 degrees to the bearing of the points based on mouse position
PointLatLngAlt p0;
PointLatLngAlt p1;
PointLatLngAlt bestp0 = PointLatLngAlt.Zero;
PointLatLngAlt bestp1 = PointLatLngAlt.Zero;
double bestcrosstrack = 9999999;
double R = 6371000;
for (int a = 0; a < boxpoly.Points.Count; a++)
{
p0 = boxpoly.Points[a];
p1 = boxpoly.Points[(a + 1) % (boxpoly.Points.Count)];
var distp0p1 = p0.GetDistance(mousecurrent);
var bearingp0curr = p0.GetBearing(mousecurrent);
var bearringp0p1 = p0.GetBearing(p1);
var ct = Math.Asin(Math.Sin(distp0p1 / R) * Math.Sin((bearingp0curr - bearringp0p1) * deg2rad)) * R;
if (Math.Abs(ct) < Math.Abs(bestcrosstrack))
{
bestp0 = p0;
bestp1 = p1;
bestcrosstrack = ct;
}
}
var bearing = bestp0.GetBearing(bestp1);
layerpolygons.Markers.Clear();
layerpolygons.Markers.Add(new GMarkerGoogle(bestp0, GMarkerGoogleType.blue));
layerpolygons.Markers.Add(new GMarkerGoogle(bestp1, GMarkerGoogleType.blue));
bearing = ((PointLatLngAlt)mousestart).GetBearing(mousecurrent);
var newposp0 = bestp0.newpos(bearing, Math.Abs(bestcrosstrack));
var newposp1 = bestp1.newpos(bearing, Math.Abs(bestcrosstrack));
boxpoly.Points[boxpoly.Points.IndexOf(bestp0)] = newposp0;
boxpoly.Points[boxpoly.Points.IndexOf(bestp1)] = newposp1;
UpdateListFromBox();
map.UpdatePolygonLocalPosition(boxpoly);
map.Invalidate();
mousestart = mousecurrent;
}
}
mousedragging = false;
}
private void domainUpDown1_ValueChanged(object sender, EventArgs e)
{
if (CMB_camera.Text != "")
{
doCalc();
}
// new grid system test
List <PointLatLngAlt> list = new List <PointLatLngAlt>();
plugin.Host.FPDrawnPolygon.Points.ForEach(x => { list.Add(x); });
grid = Grid.CreateGrid(list, (double)NUM_altitude.Value, (double)NUM_Distance.Value, (double)NUM_spacing.Value, (double)NUM_angle.Value, (double)NUM_overshoot.Value, (double)NUM_overshoot2.Value, (Grid.StartPosition)Enum.Parse(typeof(Grid.StartPosition), CMB_startfrom.Text), false);
List <PointLatLng> list2 = new List <PointLatLng>();
grid.ForEach(x => { list2.Add(x); });
map.HoldInvalidation = true;
layerpolygons.Polygons.Clear();
layerpolygons.Markers.Clear();
if (chk_boundary.Checked)
{
AddDrawPolygon();
}
int a = 1;
PointLatLngAlt prevpoint = grid[0];
foreach (var item in grid)
{
if (item.Tag == "M")
{
if (chk_internals.Checked)
{
layerpolygons.Markers.Add(new GMapMarkerGoogleGreen(item)
{
ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.OnMouseOver
});
}
try
{
if (TXT_fovH.Text != "")
{
float fovh = float.Parse(TXT_fovH.Text);
float fovv = float.Parse(TXT_fovV.Text);
float startangle = 0;
if (!CHK_camdirection.Checked)
{
startangle = 90;
}
float angle1 = startangle - (float)(Math.Tan((fovv / 2) / (fovh / 2)) * rad2deg);
float dist1 = (float)Math.Sqrt(Math.Pow(fovh / 2, 2) + Math.Pow(fovv / 2, 2));
float bearing = (float)prevpoint.GetBearing(item);
List <PointLatLng> footprint = new List <PointLatLng>();
footprint.Add(item.newpos(bearing + angle1, dist1));
footprint.Add(item.newpos(bearing + 180 - angle1, dist1));
footprint.Add(item.newpos(bearing + 180 + angle1, dist1));
footprint.Add(item.newpos(bearing - angle1, dist1));
GMapPolygon poly = new GMapPolygon(footprint, a.ToString());
poly.Stroke.Color = Color.FromArgb(250 - ((a * 5) % 240), 250 - ((a * 3) % 240), 250 - ((a * 9) % 240));
poly.Stroke.Width = 1;
poly.Fill = new SolidBrush(Color.FromArgb(40, Color.Purple));
if (chk_footprints.Checked)
{
layerpolygons.Polygons.Add(poly);
}
}
}
catch { }
}
else
{
if (chk_markers.Checked)
{
layerpolygons.Markers.Add(new GMapMarkerGoogleGreen(item)
{
ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.Always
});
}
}
prevpoint = item;
a++;
}
// add wp polygon
wppoly = new GMapPolygon(list2, "Grid");
wppoly.Stroke.Color = Color.Yellow;
wppoly.Stroke.Width = 4;
if (chk_grid.Checked)
{
layerpolygons.Polygons.Add(wppoly);
}
Console.WriteLine("Poly Dist " + wppoly.Distance);
lbl_area.Text = calcpolygonarea(plugin.Host.FPDrawnPolygon.Points).ToString("#") + " m^2";
lbl_distance.Text = wppoly.Distance.ToString("0.##") + " km";
lbl_spacing.Text = NUM_spacing.Value.ToString("#") + " m";
lbl_grndres.Text = TXT_cmpixel.Text;
map.HoldInvalidation = false;
map.ZoomAndCenterMarkers("polygons");
}
