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); }
List <PointLatLngAlt> getSRTMAltPath(List <PointLatLngAlt> list) { List <PointLatLngAlt> answer = new List <PointLatLngAlt>(); PointLatLngAlt last = null; double disttotal = 0; foreach (PointLatLngAlt loc in list) { if (loc == null) { continue; } if (last == null) { last = loc; continue; } double dist = last.GetDistance(loc); int points = (int)(dist / 10) + 1; double deltalat = (last.Lat - loc.Lat); double deltalng = (last.Lng - loc.Lng); double steplat = deltalat / points; double steplng = deltalng / points; PointLatLngAlt lastpnt = last; for (int a = 0; a <= points; a++) { double lat = last.Lat - steplat * a; double lng = last.Lng - steplng * a; var newpoint = new PointLatLngAlt(lat, lng, srtm.getAltitude(lat, lng).alt, ""); double subdist = lastpnt.GetDistance(newpoint); disttotal += subdist; // srtm alts list3.Add(disttotal, newpoint.Alt / CurrentState.multiplierdist); // terrain alt list4terrain.Add(disttotal, (newpoint.Alt - homealt + loc.Alt) / CurrentState.multiplierdist); lastpnt = newpoint; } answer.Add(new PointLatLngAlt(loc.Lat, loc.Lng, srtm.getAltitude(loc.Lat, loc.Lng).alt, "")); last = loc; } return(answer); }
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); }
private List <PointLatLngAlt> GetLocations(List <PointLatLngAlt> path, PointLatLngAlt location, double lead, double seperation) { List <PointLatLngAlt> result = new List <PointLatLngAlt>(); // get the current location closest point PointLatLngAlt closestPointLatLngAlt = PointLatLngAlt.Zero; double mindist = 99999999; foreach (var pointLatLngAlt in path) { var distloc = location.GetDistance(pointLatLngAlt); if (distloc < mindist) { mindist = distloc; closestPointLatLngAlt = pointLatLngAlt; } } var start = path.IndexOf(closestPointLatLngAlt); var a = 0; for (int i = start; i < (path.Count - 1); i++) { var targetdistance = lead - a * seperation; if (targetdistance < 0) { i -= 2; } if (i < 0) { break; } double dist = closestPointLatLngAlt.GetDistance(path[i]); // 2d distance if (dist >= Math.Abs(targetdistance)) { result.Add(path[i]); i = start; if (result.Count > 20) { break; } if (seperation == 0) { break; } a++; } } return(result); }
PointLatLngAlt FindTrailPnt(PointLatLngAlt from) { // get the start point for the distance int start = trail.IndexOf(from); for (int i = start+1; i < trail.Count; i++) { double dist = from.GetDistance(trail[i]); // 2d distance if (dist > FollowDistance) { return trail[i]; } } return null; }
PointLatLngAlt FindTrailPnt(PointLatLngAlt from) { // get the start point for the distance int start = trail.IndexOf(from); for (int i = start; i > 0; i--) { double dist = from.GetDistance(trail[i]); // 2d distance if (dist > Seperation) { return(trail[i]); } } return(null); }
PointLatLngAlt FindTrailPnt(PointLatLngAlt from) { // get the start point for the distance int start = trail.IndexOf(from); for (int i = start + 1; i < trail.Count; i++) { double dist = from.GetDistance(trail[i]); // 2d distance if (dist > FollowDistance) { return(trail[i]); } } return(null); }
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); }
public int get_closest_pole() { double distance = Double.MaxValue; int closest_pole = -1; int index = 0; foreach (PointLatLngAlt p in poles) { double d = copter_position.GetDistance(p); if (d < distance) { distance = d; closest_pole = index; } index++; } return(closest_pole); }
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 override bool Loop() { // display to console if (display.AddSeconds(5) < DateTime.Now) { Console.WriteLine(stats.ToString()); display = DateTime.Now; } // connects if (connectedstate != Host.comPort.BaseStream.IsOpen) { if (Host.comPort.BaseStream.IsOpen == true) { stats.lastconnect = DateTime.Now; stats.connects++; connectedstate = true; } else { stats.lastdisconnect = DateTime.Now; connectedstate = false; } } // if we are not connected, dont do anything if (!Host.comPort.BaseStream.IsOpen) { return(true); } // armed time if (Host.cs.armed) { stats.armedtime++; } // distance traveled if (Host.cs.armed && Host.cs.gpsstatus == 3 && (Host.cs.ch3percent > 12 || Host.cs.groundspeed > 3.0)) { stats.timeInAir++; if (lastpos != null && lastpos.Lat != 0 && lastpos.Lng != 0) { stats.distTraveledmeters += lastpos.GetDistance(new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, "")); lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, ""); } else { lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, ""); } } // altitude gained if (Host.cs.armed && Host.cs.gpsstatus == 3) { stats.maxalt = Math.Max(Host.cs.altasl, stats.maxalt); stats.minalt = Math.Min(Host.cs.altasl, stats.minalt); stats.maxspeed = Math.Max(Host.cs.groundspeed, stats.maxspeed); stats.avgspeed = Host.cs.groundspeed; } // gps lock time if (Host.cs.gpsstatus == 3) { stats.gpslocktime++; } if (Host.cs.battery_usedmah > 0) { stats.mahused += Host.cs.battery_usedmah - lastmahused; lastmahused = Host.cs.battery_usedmah; } else { lastmahused = 0; } // bytes received //stats["bytesreceived"] += Host.comPort.BytesReceived.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum()); // bytes sent //stats["bytessent"] += Host.comPort.BytesSent.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum()); // connect time if (Host.comPort.BaseStream.IsOpen) { stats.connectedtime++; } return(true); }
public override void SendCommand() { if (masterpos.Lat == 0 || masterpos.Lng == 0) { return; } //Console.WriteLine(DateTime.Now); //Console.WriteLine("Leader {0} {1} {2}", masterpos.Lat, masterpos.Lng, masterpos.Alt); int a = 0; foreach (var port in MainV2.Comports.ToArray()) { foreach (var mav in port.MAVlist) { if (mav == Leader) { continue; } PointLatLngAlt target = new PointLatLngAlt(masterpos); try { int utmzone = (int)((masterpos.Lng - -186.0) / 6.0); IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone, masterpos.Lat < 0 ? false : true); ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm); double[] pll1 = { target.Lng, target.Lat }; double[] p1 = trans.MathTransform.Transform(pll1); double heading = -Leader.cs.yaw; double length = offsets[mav].length(); var x = ((HIL.Vector3)offsets[mav]).x; var y = ((HIL.Vector3)offsets[mav]).y; // add offsets to utm p1[0] += x * Math.Cos(heading * MathHelper.deg2rad) - y * Math.Sin(heading * MathHelper.deg2rad); p1[1] += x * Math.Sin(heading * MathHelper.deg2rad) + y * Math.Cos(heading * MathHelper.deg2rad); if (mav.cs.firmware == MainV2.Firmwares.ArduPlane) { // project the point forwards gs*5 var gs = mav.cs.groundspeed * 5; p1[1] += gs * Math.Cos((-heading) * MathHelper.deg2rad); p1[0] += gs * Math.Sin((-heading) * MathHelper.deg2rad); } // convert back to wgs84 IMathTransform inversedTransform = trans.MathTransform.Inverse(); double[] point = inversedTransform.Transform(p1); target.Lat = point[1]; target.Lng = point[0]; target.Alt += ((HIL.Vector3)offsets[mav]).z; if (mav.cs.firmware == MainV2.Firmwares.ArduPlane) { var dist = target.GetDistance(new PointLatLngAlt(mav.cs.lat, mav.cs.lng, mav.cs.alt)); dist -= mav.cs.groundspeed * 5; var leadergs = Leader.cs.groundspeed; var newspeed = (leadergs + (float)(dist / 10)); if (newspeed < 5) { newspeed = 5; } port.setParam(mav.sysid, mav.compid, "TRIM_ARSPD_CM", newspeed * 100.0f); // send position port.setGuidedModeWP(mav.sysid, mav.compid, new Locationwp() { alt = (float)target.Alt, lat = target.Lat, lng = target.Lng, id = (ushort)MAVLink.MAV_CMD.WAYPOINT }); } else { Vector3 vel = new Vector3(Leader.cs.vx, Leader.cs.vy, Leader.cs.vz); // do pos/vel port.setPositionTargetGlobalInt(mav.sysid, mav.compid, true, true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, target.Lat, target.Lng, target.Alt, vel.x, vel.y, -vel.z, 0, 0); // do yaw if (!gimbal) { // within 3 degrees dont send if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3) { port.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.CONDITION_YAW, Leader.cs.yaw, 100.0f, 0, 0, 0, 0, 0, false); } } else { // gimbal direction if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3) { port.setMountControl(mav.sysid, mav.compid, 45, 0, Leader.cs.yaw, false); } } } //Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt); } catch (Exception ex) { Console.WriteLine("Failed to send command " + mav.ToString() + "\n" + ex.ToString()); } a++; } } }
private void RegenerateWPRoute(List <PointLatLngAlt> fullpointlist, PointLatLngAlt HomeLocation) { route.Clear(); homeroute.Clear(); PointLatLngAlt lastpnt = fullpointlist[0]; PointLatLngAlt lastpnt2 = fullpointlist[0]; PointLatLngAlt lastnonspline = fullpointlist[0]; List <PointLatLngAlt> splinepnts = new List <PointLatLngAlt>(); List <PointLatLngAlt> wproute = new List <PointLatLngAlt>(); // add home - this causeszx the spline to always have a straight finish fullpointlist.Add(fullpointlist[0]); for (int a = 0; a < fullpointlist.Count; a++) { if (fullpointlist[a] == null) { continue; } if (fullpointlist[a].Tag2 == "spline") { if (splinepnts.Count == 0) { splinepnts.Add(lastpnt); } splinepnts.Add(fullpointlist[a]); } else { if (splinepnts.Count > 0) { List <PointLatLng> list = new List <PointLatLng>(); splinepnts.Add(fullpointlist[a]); Spline2 sp = new Spline2(HomeLocation); sp.set_wp_origin_and_destination(sp.pv_location_to_vector(lastpnt2), sp.pv_location_to_vector(lastpnt)); sp._flags.reached_destination = true; for (int no = 1; no < (splinepnts.Count - 1); no++) { Spline2.spline_segment_end_type segtype = Spline2.spline_segment_end_type.SEGMENT_END_STRAIGHT; if (no < (splinepnts.Count - 2)) { segtype = Spline2.spline_segment_end_type.SEGMENT_END_SPLINE; } sp.set_spline_destination(sp.pv_location_to_vector(splinepnts[no]), false, segtype, sp.pv_location_to_vector(splinepnts[no + 1])); //sp.update_spline(); while (sp._flags.reached_destination == false) { float t = 1f; //sp.update_spline(); sp.advance_spline_target_along_track(t); // Console.WriteLine(sp.pv_vector_to_location(sp.target_pos).ToString()); list.Add(sp.pv_vector_to_location(sp.target_pos)); } list.Add(splinepnts[no]); } list.ForEach(x => { wproute.Add(x); }); splinepnts.Clear(); lastnonspline = fullpointlist[a]; } wproute.Add(fullpointlist[a]); lastpnt2 = lastpnt; lastpnt = fullpointlist[a]; } } int count = wproute.Count; int counter = 0; PointLatLngAlt homepoint = new PointLatLngAlt(); PointLatLngAlt firstpoint = new PointLatLngAlt(); PointLatLngAlt lastpoint = new PointLatLngAlt(); if (count > 2) { // homeroute = last, home, first wproute.ForEach(x => { counter++; if (counter == 1) { homepoint = x; return; } if (counter == 2) { firstpoint = x; } if (counter == count - 1) { lastpoint = x; } if (counter == count) { homeroute.Points.Add(lastpoint); homeroute.Points.Add(homepoint); homeroute.Points.Add(firstpoint); return; } route.Points.Add(x); }); homeroute.Stroke = new Pen(Color.Yellow, 2); // if we have a large distance between home and the first/last point, it hangs on the draw of a the dashed line. if (homepoint.GetDistance(lastpoint) < 5000 && homepoint.GetDistance(firstpoint) < 5000) { homeroute.Stroke.DashStyle = DashStyle.Dash; } overlay.Routes.Add(homeroute); route.Stroke = new Pen(Color.Yellow, 4); route.Stroke.DashStyle = DashStyle.Custom; overlay.Routes.Add(route); } }
void processbg(string file) { a++; Loading.ShowLoading(a + "/" + files.Count + " " + file, this); if (!File.Exists(file + ".jpg")) { LogMap.MapLogs(new string[] { file }); } var loginfo = new loginfo(); loginfo.fullname = file; try { // file not found exception even though it passes the exists check above. loginfo.Size = new FileInfo(file).Length; } catch { } if (File.Exists(file + ".jpg")) { loginfo.imgfile = file + ".jpg"; } if (file.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) { try { mine.logplaybackfile = new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; } mine.logreadmode = true; mine.speechenabled = false; // file is to small if (mine.logplaybackfile.BaseStream.Length < 1024 * 4) { return; } mine.getHeartBeat(); loginfo.Date = mine.lastlogread; loginfo.Aircraft = mine.sysidcurrent; loginfo.Frame = mine.MAV.aptype.ToString(); var start = mine.lastlogread; try { mine.logplaybackfile.BaseStream.Seek(0, SeekOrigin.Begin); } catch { } var end = mine.lastlogread; var length = mine.logplaybackfile.BaseStream.Length; var a = 0; // abandon last 100 bytes while (mine.logplaybackfile.BaseStream.Position < (length - 100)) { var packet = mine.readPacket(); // gcs if (packet.sysid == 255) { continue; } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAMERA_FEEDBACK) { loginfo.CamMSG++; } if (a % 10 == 0) { mine.MAV.cs.UpdateCurrentSettings(null, true, mine); } a++; if (mine.lastlogread > end) { end = mine.lastlogread; } } loginfo.Home = mine.MAV.cs.Location; loginfo.TimeInAir = mine.MAV.cs.timeInAir; loginfo.DistTraveled = mine.MAV.cs.distTraveled; loginfo.Duration = (end - start).ToString(); } } else if (file.ToLower().EndsWith(".bin") || file.ToLower().EndsWith(".log")) { using (DFLogBuffer colbuf = new DFLogBuffer(File.OpenRead(file))) { PointLatLngAlt lastpos = null; DateTime start = DateTime.MinValue; DateTime end = DateTime.MinValue; DateTime tia = DateTime.MinValue; // set time in air/home/distancetraveled foreach (var dfItem in colbuf.GetEnumeratorType("GPS")) { if (dfItem["Status"] != null) { var status = int.Parse(dfItem["Status"]); if (status >= 3) { var pos = new PointLatLngAlt( double.Parse(dfItem["Lat"], CultureInfo.InvariantCulture), double.Parse(dfItem["Lng"], CultureInfo.InvariantCulture), double.Parse(dfItem["Alt"], CultureInfo.InvariantCulture)); if (lastpos == null) { lastpos = pos; } if (start == DateTime.MinValue) { loginfo.Date = dfItem.time; start = dfItem.time; } end = dfItem.time; // add distance loginfo.DistTraveled += (float)lastpos.GetDistance(pos); // set home if (loginfo.Home == null) { loginfo.Home = pos; } if (dfItem.time > tia.AddSeconds(1)) { // ground speed > 0.2 or alt > homelat+2 if (double.Parse(dfItem["Spd"], CultureInfo.InvariantCulture) > 0.2 || pos.Alt > (loginfo.Home.Alt + 2)) { loginfo.TimeInAir++; } tia = dfItem.time; } } } } loginfo.Duration = (end - start).ToString(); loginfo.CamMSG = colbuf.GetEnumeratorType("CAM").Count(); loginfo.Aircraft = 0; //colbuf.dflog.param[""]; loginfo.Frame = "Unknown"; //mine.MAV.aptype.ToString(); } } lock (logs) logs.Add(loginfo); }
float testCode(MAVState MAV, PointLatLngAlt Location) { // this is GeoFenceDist from currentstate try { float disttotal = 99999; var R = 6371e3; var currenthash = MAV.fencepoints.GetHashCode(); lock (this) if (currenthash != listhash) { list = MAV.fencepoints .Where(a => a.Value.command != (ushort)MAVLink.MAV_CMD.FENCE_RETURN_POINT) .ChunkByField((a, b, count) => { // these fields types stand alone if (a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION || a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION) { return(false); } if (count >= b.Value.param1) { return(false); } return(a.Value.command == b.Value.command); }).ToList(); listhash = currenthash; } // check all sublists foreach (var sublist in list) { // process circles if (sublist.Count() == 1) { var item = sublist.First().Value; if (item.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION) { var lla = new PointLatLngAlt(item.x / 1e7, item.y / 1e7); var dist = lla.GetDistance(Location); if (dist < item.param1) { return(0); } disttotal = (float)Math.Min(dist - item.param1, disttotal); } else if (item.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION) { var lla = new PointLatLngAlt(item.x / 1e7, item.y / 1e7); var dist = lla.GetDistance(Location); if (dist > item.param1) { return(0); } disttotal = (float)Math.Min(item.param1 - dist, disttotal); } } if (sublist == null || sublist.Count() < 3) { continue; } if (PolygonTools.isInside( sublist.CloseLoop().Select(a => new PolygonTools.Point(a.Value.y / 1e7, a.Value.x / 1e7)).ToList(), new PolygonTools.Point(Location.Lng, Location.Lat))) { if (sublist.First().Value.command == (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION) { return(0); } } else { if (sublist.First().Value.command == (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION) { return(0); } } PointLatLngAlt lineStartLatLngAlt = null; // check all segments foreach (var mavlinkFencePointT in sublist.CloseLoop()) { if (lineStartLatLngAlt == null) { lineStartLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7, mavlinkFencePointT.Value.y / 1e7); continue; } // crosstrack distance var lineEndLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7, mavlinkFencePointT.Value.y / 1e7); var lineDist = lineStartLatLngAlt.GetDistance2(lineEndLatLngAlt); var distToLocation = lineStartLatLngAlt.GetDistance2(Location); var bearToLocation = lineStartLatLngAlt.GetBearing(Location); var lineBear = lineStartLatLngAlt.GetBearing(lineEndLatLngAlt); var angle = bearToLocation - lineBear; if (angle < 0) { angle += 360; } var alongline = Math.Cos(angle * MathHelper.deg2rad) * distToLocation; // check to see if our point is still within the line length if (alongline < 0 || alongline > lineDist) { lineStartLatLngAlt = lineEndLatLngAlt; continue; } var dXt2 = Math.Sin(angle * MathHelper.deg2rad) * distToLocation; var dXt = Math.Asin(Math.Sin(distToLocation / R) * Math.Sin(angle * MathHelper.deg2rad)) * R; disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2)); lineStartLatLngAlt = lineEndLatLngAlt; } } // check also distance from the points - because if we are outside the polygon, we may be on a corner segment foreach (var sublist in list) { foreach (var mavlinkFencePointT in sublist) { if (mavlinkFencePointT.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION) { continue; } var pathpoint = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7, mavlinkFencePointT.Value.y / 1e7); var dXt2 = pathpoint.GetDistance(Location); disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2)); } } return(disttotal); } catch { return(0); } }
void dostats(whattostat stats) { // connects if (connectedstate != Host.comPort.BaseStream.IsOpen) { if (Host.comPort.BaseStream.IsOpen == true) { stats.lastconnect = DateTime.Now; stats.connects++; connectedstate = true; } else { stats.lastdisconnect = DateTime.Now; connectedstate = false; } } // if we are not connected, dont do anything if (!Host.comPort.BaseStream.IsOpen) { return; } // armed time if (Host.cs.armed) { stats.armedtime++; } // distance traveled if (Host.cs.armed && Host.cs.gpsstatus >= 3 && (Host.cs.ch3percent > 12 || Host.cs.groundspeed > 3.0)) { stats.timeInAir++; if (lastpos != null && lastpos.Lat != 0 && lastpos.Lng != 0) { double dist = lastpos.GetDistance(new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, "")); // max jump size is 400 m if (dist < 400) { stats.distTraveledmeters += dist; } lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, ""); } else { lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, ""); } } // altitude gained if (Host.cs.armed && Host.cs.gpsstatus >= 3) { stats.maxalt = Math.Max(Host.cs.altasl, stats.maxalt); stats.minalt = Math.Min(Host.cs.altasl, stats.minalt); stats.maxspeed = Math.Max(Host.cs.groundspeed, stats.maxspeed); stats.avgspeed = Host.cs.groundspeed; } // gps lock time if (Host.cs.gpsstatus >= 3) { stats.gpslocktime++; } if (Host.cs.battery_usedmah > 0) { stats.mahused += Host.cs.battery_usedmah - lastmahused; lastmahused = Host.cs.battery_usedmah; } else { lastmahused = 0; } // bytes received //stats["bytesreceived"] += Host.comPort.BytesReceived.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum()); // bytes sent //stats["bytessent"] += Host.comPort.BytesSent.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum()); // connect time if (Host.comPort.BaseStream.IsOpen) { stats.connectedtime++; } return; }
public override void SendCommand() { if (masterpos.Lat == 0 || masterpos.Lng == 0) { return; } Console.WriteLine(DateTime.Now); Console.WriteLine("Leader {0} {1} {2}", masterpos.Lat, masterpos.Lng, masterpos.Alt); int a = 0; foreach (var port in MainV2.Comports) { if (port == Leader) { continue; } PointLatLngAlt target = new PointLatLngAlt(masterpos); try { //convert Wgs84ConversionInfo to utm CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory(); GeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84; int utmzone = (int)((masterpos.Lng - -186.0) / 6.0); IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone, masterpos.Lat < 0 ? false : true); ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm); double[] pll1 = { target.Lng, target.Lat }; double[] p1 = trans.MathTransform.Transform(pll1); double heading = -Leader.MAV.cs.yaw; double length = offsets[port].length(); var x = ((HIL.Vector3)offsets[port]).x; var y = ((HIL.Vector3)offsets[port]).y; // add offsets to utm p1[0] += x * Math.Cos(heading * deg2rad) - y * Math.Sin(heading * deg2rad); p1[1] += x * Math.Sin(heading * deg2rad) + y * Math.Cos(heading * deg2rad); if (port.MAV.cs.firmware == MainV2.Firmwares.ArduPlane) { // project the point forwards gs*5 var gs = port.MAV.cs.groundspeed; p1[1] += gs * 5 * Math.Cos((-heading) * deg2rad); p1[0] += gs * 5 * Math.Sin((-heading) * deg2rad); } // convert back to wgs84 IMathTransform inversedTransform = trans.MathTransform.Inverse(); double[] point = inversedTransform.Transform(p1); target.Lat = point[1]; target.Lng = point[0]; target.Alt += ((HIL.Vector3)offsets[port]).z; if (port.MAV.cs.firmware == MainV2.Firmwares.ArduPlane) { var dist = target.GetDistance(new PointLatLngAlt(port.MAV.cs.lat, port.MAV.cs.lng, port.MAV.cs.alt)); dist -= port.MAV.cs.groundspeed * 5; var leadergs = Leader.MAV.cs.groundspeed; var newspeed = (leadergs + (float)(dist / 10)); if (newspeed < 5) { newspeed = 5; } port.setParam("TRIM_ARSPD_CM", newspeed * 100.0f); // send position port.setGuidedModeWP(new Locationwp() { alt = (float)target.Alt, lat = target.Lat, lng = target.Lng, id = (ushort)MAVLink.MAV_CMD.WAYPOINT }); } else { Vector3 vel = new Vector3(Math.Cos(Leader.MAV.cs.groundcourse * deg2rad) * Leader.MAV.cs.groundspeed, Math.Sin(Leader.MAV.cs.groundcourse * deg2rad) * Leader.MAV.cs.groundspeed, Leader.MAV.cs.verticalspeed); port.setPositionTargetGlobalInt((byte)port.sysidcurrent, (byte)port.compidcurrent, true, true, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, target.Lat, target.Lng, target.Alt, vel.x, vel.y, -vel.z); } Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt); } catch (Exception ex) { Console.WriteLine("Failed to send command " + port.ToString() + "\n" + ex.ToString()); } a++; } }
List <PointLatLngAlt> getSRTMAltPath(List <PointLatLngAlt> list) { List <PointLatLngAlt> answer = new List <PointLatLngAlt>(); PointLatLngAlt last = null; double disttotal = 0; foreach (PointLatLngAlt loc in list) { if (loc == null) { continue; } if (last == null) { last = loc; if (altmode == FlightPlanner.altmode.Terrain || altmode2 == FlightPlanner2.altmode.Terrain) { loc.Alt -= srtm.getAltitude(loc.Lat, loc.Lng).alt; } continue; } double dist = last.GetDistance(loc); if (altmode == FlightPlanner.altmode.Terrain || altmode2 == FlightPlanner2.altmode.Terrain) { loc.Alt -= srtm.getAltitude(loc.Lat, loc.Lng).alt; } int points = (int)(dist / 10) + 1; double deltalat = (last.Lat - loc.Lat); double deltalng = (last.Lng - loc.Lng); double deltaalt = last.Alt - loc.Alt; double steplat = deltalat / points; double steplng = deltalng / points; double stepalt = deltaalt / points; PointLatLngAlt lastpnt = last; for (int a = 0; a <= points; a++) { double lat = last.Lat - steplat * a; double lng = last.Lng - steplng * a; double alt = last.Alt - stepalt * a; var newpoint = new PointLatLngAlt(lat, lng, srtm.getAltitude(lat, lng).alt, ""); double subdist = lastpnt.GetDistance(newpoint); disttotal += subdist; // srtm alts list3.Add(disttotal * CurrentState.multiplierdist, Convert.ToInt32(newpoint.Alt * CurrentState.multiplieralt)); // terrain alt list4terrain.Add(disttotal * CurrentState.multiplierdist, Convert.ToInt32((newpoint.Alt + alt) * CurrentState.multiplieralt)); lastpnt = newpoint; } answer.Add(new PointLatLngAlt(loc.Lat, loc.Lng, srtm.getAltitude(loc.Lat, loc.Lng).alt, "")); last = loc; } return(answer); }
public override void SendCommand() { if (masterpos.Lat == 0 || masterpos.Lng == 0) { return; } //Console.WriteLine(DateTime.Now); //Console.WriteLine("Leader {0} {1} {2}", masterpos.Lat, masterpos.Lng, masterpos.Alt); int a = 0; foreach (var port in MainV2.Comports.ToArray()) { foreach (var mav in port.MAVlist) { if (mav == Leader) { continue; } PointLatLngAlt target = new PointLatLngAlt(masterpos); try { int utmzone = (int)((masterpos.Lng - -186.0) / 6.0); IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone, masterpos.Lat < 0 ? false : true); ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm); double[] pll1 = { target.Lng, target.Lat }; double[] p1 = trans.MathTransform.Transform(pll1); double heading = -Leader.cs.yaw; double length = offsets[mav].length(); var x = ((Vector3)offsets[mav]).x; var y = ((Vector3)offsets[mav]).y; // add offsets to utm p1[0] += x * Math.Cos(heading * MathHelper.deg2rad) - y * Math.Sin(heading * MathHelper.deg2rad); p1[1] += x * Math.Sin(heading * MathHelper.deg2rad) + y * Math.Cos(heading * MathHelper.deg2rad); // convert back to wgs84 IMathTransform inversedTransform = trans.MathTransform.Inverse(); double[] point = inversedTransform.Transform(p1); target.Lat = point[1]; target.Lng = point[0]; target.Alt += ((Vector3)offsets[mav]).z; if (mav.cs.firmware == Firmwares.ArduPlane) { // get distance from target position var dist = target.GetDistance(mav.cs.Location); // get bearing to target var targyaw = mav.cs.Location.GetBearing(target); var targettrailer = target.newpos(Leader.cs.yaw, Math.Abs(dist) * -0.25); var targetleader = target.newpos(Leader.cs.yaw, 10 + dist); var yawerror = wrap_180(targyaw - mav.cs.yaw); var mavleadererror = wrap_180(Leader.cs.yaw - mav.cs.yaw); if (dist < 100) { targyaw = mav.cs.Location.GetBearing(targetleader); yawerror = wrap_180(targyaw - mav.cs.yaw); var targBearing = mav.cs.Location.GetBearing(target); // check the bearing for the leader and target are within 45 degrees. if (Math.Abs(wrap_180(targBearing - targyaw)) > 45) { dist *= -1; } } else { targyaw = mav.cs.Location.GetBearing(targettrailer); yawerror = wrap_180(targyaw - mav.cs.yaw); } // display update mav.GuidedMode.x = (float)target.Lat; mav.GuidedMode.y = (float)target.Lng; mav.GuidedMode.z = (float)target.Alt; MAVLink.mavlink_set_attitude_target_t att_target = new MAVLink.mavlink_set_attitude_target_t(); att_target.target_system = mav.sysid; att_target.target_component = mav.compid; att_target.type_mask = 0xff; Tuple <PID, PID, PID, PID> pid; if (pids.ContainsKey(mav)) { pid = pids[mav]; } else { pid = new Tuple <PID, PID, PID, PID>( new PID(1f, .03f, 0.02f, 10, 20, 0.1f, 0), new PID(1f, .03f, 0.02f, 10, 20, 0.1f, 0), new PID(1, 0, 0.00f, 15, 20, 0.1f, 0), new PID(0.01f, 0.001f, 0, 0.5f, 20, 0.1f, 0)); pids.Add(mav, pid); } var rollp = pid.Item1; var pitchp = pid.Item2; var yawp = pid.Item3; var thrustp = pid.Item4; var newroll = 0d; var newpitch = 0d; if (true) { var altdelta = target.Alt - mav.cs.alt; newpitch = altdelta; att_target.type_mask -= 0b00000010; pitchp.set_input_filter_all((float)altdelta); newpitch = pitchp.get_pid(); } if (true) { var leaderturnrad = Leader.cs.radius; var mavturnradius = leaderturnrad - x; { var distToTarget = mav.cs.Location.GetDistance(target); var bearingToTarget = mav.cs.Location.GetBearing(target); // bearing stability if (distToTarget < 30) { bearingToTarget = mav.cs.Location.GetBearing(targetleader); } // fly in from behind if (distToTarget > 100) { bearingToTarget = mav.cs.Location.GetBearing(targettrailer); } var bearingDelta = wrap_180(bearingToTarget - mav.cs.yaw); var tangent90 = bearingDelta > 0 ? 90 : -90; newroll = 0; // if the delta is > 90 then we are facing the wrong direction if (Math.Abs(bearingDelta) < 85) { var insideAngle = Math.Abs(tangent90 - bearingDelta); var angleCenter = 180 - insideAngle * 2; // sine rule var sine1 = Math.Max(distToTarget, 40) / Math.Sin(angleCenter * MathHelper.deg2rad); var radius = sine1 * Math.Sin(insideAngle * MathHelper.deg2rad); // average calced + leader offset turnradius - acts as a FF radius = (Math.Abs(radius) + Math.Abs(mavturnradius)) / 2; var angleBank = ((mav.cs.groundspeed * mav.cs.groundspeed) / radius) / 9.8; angleBank *= MathHelper.rad2deg; if (bearingDelta > 0) { newroll = Math.Abs(angleBank); } else { newroll = -Math.Abs(angleBank); } } newroll += MathHelper.constrain(bearingDelta, -20, 20); } // tr = gs2 / (9.8 * x) // (9.8 * x) * tr = gs2 // 9.8 * x = gs2 / tr // (gs2/tr)/9.8 = x var angle = ((mav.cs.groundspeed * mav.cs.groundspeed) / mavturnradius) / 9.8; //newroll = angle * MathHelper.rad2deg; // 1 degree of roll for ever 1 degree of yaw error //newroll += MathHelper.constrain(yawerror, -20, 20); //rollp.set_input_filter_all((float)yawdelta); } // do speed if (true) { //att_target.thrust = (float) MathHelper.mapConstrained(dist, 0, 40, 0, 1); att_target.type_mask -= 0b01000000; // in m out 0-1 thrustp.set_input_filter_all((float)dist); // prevent buildup prior to being close if (dist > 40) { thrustp.reset_I(); } // 0.1 demand + pid results att_target.thrust = (float)MathHelper.constrain(thrustp.get_pid(), 0.1, 1); } Quaternion q = new Quaternion(); q.from_vector312(newroll * MathHelper.deg2rad, newpitch * MathHelper.deg2rad, yawerror * MathHelper.deg2rad); att_target.q = new float[4]; att_target.q[0] = (float)q.q1; att_target.q[1] = (float)q.q2; att_target.q[2] = (float)q.q3; att_target.q[3] = (float)q.q4; //0b0= rpy att_target.type_mask -= 0b10000101; //att_target.type_mask -= 0b10000100; Console.WriteLine("sysid {0} - {1} dist {2} r {3} p {4} y {5}", mav.sysid, att_target.thrust, dist, newroll, newpitch, (targyaw - mav.cs.yaw)); /* Console.WriteLine("rpyt {0} {1} {2} {3} I {4} {5} {6} {7}", * rollp.get_pid(), pitchp.get_pid(), yawp.get_pid(), thrustp.get_pid(), * rollp.get_i(), pitchp.get_i(), yawp.get_i(), thrustp.get_i()); */ port.sendPacket(att_target, mav.sysid, mav.compid); } else { Vector3 vel = new Vector3(Leader.cs.vx, Leader.cs.vy, Leader.cs.vz); // do pos/vel port.setPositionTargetGlobalInt(mav.sysid, mav.compid, true, true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, target.Lat, target.Lng, target.Alt, vel.x, vel.y, vel.z, 0, 0); // do yaw if (!gimbal) { // within 3 degrees dont send if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3) { port.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.CONDITION_YAW, Leader.cs.yaw, 100.0f, 0, 0, 0, 0, 0, false); } } else { // gimbal direction if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3) { port.setMountControl(mav.sysid, mav.compid, 45, 0, Leader.cs.yaw, false); } } } //Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt); } catch (Exception ex) { Console.WriteLine("Failed to send command " + mav.ToString() + "\n" + ex.ToString()); } a++; } } }
float testCode(MAVState MAV, PointLatLngAlt Location) { // this is GeoFenceDist from currentstate try { float disttotal = 99999; var R = 6371e3; var list = MAV.fencepoints .Where(a => a.Value.command != (ushort)MAVLink.MAV_CMD.FENCE_RETURN_POINT) .ChunkByField((a, b, count) => { // these fields types stand alone if (a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION || a.Value.command == (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION) { return(false); } if (count > b.Value.param1) { return(false); } return(a.Value.command == b.Value.command); }); // check all sublists foreach (var sublist in list) { PointLatLngAlt lineStartLatLngAlt = null; // check all segments foreach (var mavlinkFencePointT in sublist.CloseLoop()) { if (lineStartLatLngAlt == null) { lineStartLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7, mavlinkFencePointT.Value.y / 1e7); continue; } // crosstrack distance var lineEndLatLngAlt = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7, mavlinkFencePointT.Value.y / 1e7); var lineDist = lineStartLatLngAlt.GetDistance2(lineEndLatLngAlt); var distToLocation = lineStartLatLngAlt.GetDistance2(Location); var bearToLocation = lineStartLatLngAlt.GetBearing(Location); var lineBear = lineStartLatLngAlt.GetBearing(lineEndLatLngAlt); var angle = bearToLocation - lineBear; if (angle < 0) { angle += 360; } var alongline = Math.Cos(angle * MathHelper.deg2rad) * distToLocation; // check to see if our point is still within the line length if (alongline < 0 || alongline > lineDist) { lineStartLatLngAlt = lineEndLatLngAlt; continue; } var dXt2 = Math.Sin(angle * MathHelper.deg2rad) * distToLocation; var dXt = Math.Asin(Math.Sin(distToLocation / R) * Math.Sin(angle * MathHelper.deg2rad)) * R; disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2)); lineStartLatLngAlt = lineEndLatLngAlt; } } // check also distance from the points - because if we are outside the polygon, we may be on a corner segment foreach (var sublist in list) { foreach (var mavlinkFencePointT in sublist) { var pathpoint = new PointLatLngAlt(mavlinkFencePointT.Value.x / 1e7, mavlinkFencePointT.Value.y / 1e7); var dXt2 = pathpoint.GetDistance(Location); disttotal = (float)Math.Min(disttotal, Math.Abs(dXt2)); } } return(disttotal); } catch { return(0); } }
public override bool Loop() { // connects if (connectedstate != Host.comPort.BaseStream.IsOpen) { if (Host.comPort.BaseStream.IsOpen == true) { stats["connects"]++; connectedstate = true; } } // if we are not connected, dont do anything if (!Host.comPort.BaseStream.IsOpen) { return(true); } // armed time if (Host.cs.armed) { stats["armed"]++; } // distance traveled if (Host.cs.armed && Host.cs.gpsstatus == 3) { if (lastpos.Lat != 0 && lastpos.Lng != 0 && Host.cs.armed) { stats["distTraveled"] += (ulong)lastpos.GetDistance(new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, "")); lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, ""); } else { lastpos = new PointLatLngAlt(Host.cs.lat, Host.cs.lng, Host.cs.altasl, ""); } } // altitude gained if (Host.cs.armed && Host.cs.gpsstatus == 3) { stats["maxalt"] = (ulong)Host.cs.altasl; stats["minalt"] = (ulong)Host.cs.altasl; } // gps lock time if (Host.cs.gpsstatus == 3) { stats["gpslock"]++; } // time in air if (Host.cs.ch3percent > 12 || Host.cs.groundspeed > 3.0) { stats["timeInAir"]++; } // bytes received //stats["bytesreceived"] += Host.comPort.BytesReceived.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum()); // bytes sent //stats["bytessent"] += Host.comPort.BytesSent.Buffer(TimeSpan.FromSeconds(1)).Select(bytes => bytes.Sum()); // connect time if (Host.comPort.BaseStream.IsOpen) { stats["connectedtime"]++; } return(true); }
//http://www.chrobotics.com/library/understanding-euler-angles // x = north / roll // y = pitch / east // z = yaw / down public static List <PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov) { // alt should be asl // P pitch where the center os pointing ie -80 // R roll // if roll and pitch is 0, use the quick method. if (R == 0 && P == 0) { // calc fov in m on the ground at 0 alt var fovh = Math.Tan(hfov / 2.0 * MathHelper.deg2rad) * plla.Alt; var fovv = Math.Tan(vfov / 2.0 * MathHelper.deg2rad) * plla.Alt; var fovd = Math.Sqrt(fovh * fovh + fovv * fovv); // where we put our footprint var ans2 = new List <PointLatLngAlt>(); // calc bearing from center to corner var bearing1 = Math.Atan2(fovh, fovv) * MathHelper.rad2deg; // calc first corner point var newpos1 = plla.newpos(bearing1 + Y, Math.Sqrt(fovh * fovh + fovv * fovv)); // set alt to 0, as we used the hypot for distance and fov is based on 0 alt newpos1.Alt = 0; // calc intersection from center to new point and return ground hit point var cen1 = calcIntersection(plla, newpos1); // add to our footprint poly ans2.Add(cen1); addtomap(cen1, "cen1"); //repeat newpos1 = plla.newpos(Y - bearing1, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen2"); newpos1 = plla.newpos(bearing1 + Y - 180, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen3"); newpos1 = plla.newpos(Y - bearing1 - 180, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen4"); addtomap(plla, "plane"); return(ans2); } GMapPolygon poly = new GMapPolygon(new List <PointLatLng>(), "rect"); double frontangle = (P * 0) + vfov / 2; double backangle = (P * 0) - vfov / 2; double leftangle = (R * 0) + hfov / 2; double rightangle = (R * 0) - hfov / 2; addtomap(plla, "plane"); Matrix3 dcm = new Matrix3(); dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.normalize(); Vector3 center1 = new Vector3(0, 0, 10000); var test = dcm * center1; var bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var cen = calcIntersection(plla, newpos2); var dist = plla.GetDistance(cen); addtomap(cen, "center " + dist.ToString("0")); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(rightangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * MathHelper.deg2rad, 0)); dcm.normalize(); /* * var mx = new Matrix3(); * var my = new Matrix3(); * var mz = new Matrix3(); * * mx.from_euler((rightangle + R) * MathHelper.deg2rad, 0, 0); * my.from_euler(0, (frontangle + P) * MathHelper.deg2rad, 0); * mz.from_euler(0, 0, Y * MathHelper.deg2rad); * * printdcm(mx); * printdcm(my); * printdcm(mz); * printdcm(my * mx); * printdcm(mz * my * mx); * * test = mz * my * mx * center1; */ test = dcm * center1; bearing2 = (Math.Atan2(test.y, test.x) * MathHelper.rad2deg); newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; //addtomap(newpos2, "tr2"); var groundpointtr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtr); addtomap(groundpointtr, "tr"); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(leftangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointtl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtl); addtomap(groundpointtl, "tl"); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(leftangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbl); addtomap(groundpointbl, "bl"); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(rightangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbr); addtomap(groundpointbr, "br"); // polygons.Polygons.Clear(); polygons.Polygons.Add(poly); var ans = new List <PointLatLngAlt>(); ans.Add(groundpointtl); ans.Add(groundpointtr); ans.Add(groundpointbr); ans.Add(groundpointbl); return(ans); }