diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index d229a5f1bed401277b2302af45899a9a42018b64..215231803afc5cf8248dd903bd64f4f6ba5d683b 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -174,10 +174,6 @@ <SpecificVersion>False</SpecificVersion> <HintPath>Lib\OpenTK.dll</HintPath> </Reference> - <Reference Include="OpenTK.Compatibility, Version=1.0.0.0, Culture=neutral, PublicKeyToken=bad199fe84eb3df4, processorArchitecture=MSIL"> - <SpecificVersion>False</SpecificVersion> - <HintPath>..\..\..\..\..\Desktop\DIYDrones\opentk\trunk\Binaries\OpenTK\Debug\OpenTK.Compatibility.dll</HintPath> - </Reference> <Reference Include="OpenTK.GLControl, Version=1.0.0.0, Culture=neutral, PublicKeyToken=bad199fe84eb3df4, processorArchitecture=MSIL"> <SpecificVersion>False</SpecificVersion> </Reference> diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj.user b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj.user index 730cb9c5263c7a71c71470da88a8854b36e03849..e53d971127883cda877b9f96c4c042dbbbad1f2b 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj.user +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj.user @@ -11,6 +11,6 @@ <UpdateUrlHistory /> </PropertyGroup> <PropertyGroup> - <ReferencePath>C:\Users\hog\Documents\Visual Studio 2010\Projects\ArdupilotMega\ArdupilotMega\Lib\;C:\Users\hog\Desktop\DIYDrones\myquad\greatmaps_e1bb830a18a3\Demo.WindowsForms\bin\Debug\;C:\Users\hog\Desktop\DIYDrones\myquad\sharpkml\SharpKml\bin\Release\;C:\Users\hog\Desktop\DIYDrones\myquad\MetaDataExtractorCSharp240d\bin\Release\</ReferencePath> + <ReferencePath>C:\Users\hog\Desktop\DIYDrones\myquad\greatmaps_e1bb830a18a3\Demo.WindowsForms\bin\Debug\;C:\Users\hog\Documents\Visual Studio 2010\Projects\ArdupilotMega\ArdupilotMega\Lib\;C:\Users\hog\Desktop\DIYDrones\myquad\sharpkml\SharpKml\bin\Release\;C:\Users\hog\Desktop\DIYDrones\myquad\MetaDataExtractorCSharp240d\bin\Release\</ReferencePath> </PropertyGroup> </Project> \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Common.cs b/Tools/ArdupilotMegaPlanner/Common.cs index ebc4326f59ce4caec3a90c393f145442fb487526..680a8038762dda7bd614e142708e2dc9e09eb04f 100644 --- a/Tools/ArdupilotMegaPlanner/Common.cs +++ b/Tools/ArdupilotMegaPlanner/Common.cs @@ -304,6 +304,11 @@ namespace ArdupilotMega return (int)((Lat + Lng + Alt) * 100); } + public override string ToString() + { + return Lat + "," + Lng + "," + Alt; + } + /// <summary> /// Calc Distance in M /// </summary> diff --git a/Tools/ArdupilotMegaPlanner/Controls/OpenGLtest.cs b/Tools/ArdupilotMegaPlanner/Controls/OpenGLtest.cs index 9b5b9f40987b50bcbbda78f1b9f6e98d0e54ded9..b3baf42517e05e595effa66d88f26e72892b672f 100644 --- a/Tools/ArdupilotMegaPlanner/Controls/OpenGLtest.cs +++ b/Tools/ArdupilotMegaPlanner/Controls/OpenGLtest.cs @@ -6,74 +6,144 @@ using OpenTK; using OpenTK.Graphics.OpenGL; using System.Drawing.Imaging; using System.Drawing; +using GMap.NET; +using GMap.NET.WindowsForms; namespace ArdupilotMega.Controls { public class OpenGLtest : GLControl { - int fixme; + public static OpenGLtest instance; + + // terrain image Bitmap _terrain = new Bitmap(640,480); + int texture = 0; - public OpenGLtest() - { - InitializeComponent(); + float _angle = 0; + double cameraX, cameraY, cameraZ; // camera coordinates + double lookX, lookY, lookZ; // camera look-at coordinates - try - { - _terrain = new Bitmap(@"C:\Users\hog\Pictures\Denmark\[Group 1]-P1020169_P1020174-6 images.jpg"); - } - catch { } + double step = 1 / 1200.0; - _terrain = new Bitmap(_terrain, 512, 512); + RectLatLng area = new RectLatLng(-35.04286,117.84262,0.1,0.1); + double _alt = 0; + public PointLatLngAlt LocationCenter { + get { + return new PointLatLngAlt(area.LocationMiddle.Lat, area.LocationMiddle.Lng,_alt,""); + } + set { - GL.GenTextures(1, out texture); - GL.BindTexture(TextureTarget.Texture2D, texture); + if (area.LocationMiddle.Lat == value.Lat && area.LocationMiddle.Lng == value.Lng) + return; - BitmapData data = _terrain.LockBits(new System.Drawing.Rectangle(0, 0, _terrain.Width, _terrain.Height), - ImageLockMode.ReadOnly, System.Drawing.Imaging.PixelFormat.Format32bppArgb); + if (value.Lat == 0 && value.Lng == 0) + return; - //Console.WriteLine("w {0} h {1}",data.Width, data.Height); + _alt = value.Alt; + area = new RectLatLng(value.Lat + 0.15, value.Lng - 0.15, 0.3, 0.3); + // Console.WriteLine(area.LocationMiddle + " " + value.ToString()); + this.Invalidate(); + } + } - GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgba, data.Width, data.Height, 0, - OpenTK.Graphics.OpenGL.PixelFormat.Bgra, PixelType.UnsignedByte, data.Scan0); + public Vector3 rpy = new Vector3(); - _terrain.UnlockBits(data); + public OpenGLtest() + { + instance = this; - GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Linear); - GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Linear); + InitializeComponent(); + GL.GenTextures(1, out texture); } - const float rad2deg = (float)(180 / Math.PI); - const float deg2rad = (float)(1.0 / rad2deg); - - int[,] getElevationData(double lat, double lng, double direction) + void getImage() { - int[,] answer = new int[400,400]; - - double step = 0.00083333333333333; + MapType type = MapType.GoogleSatellite; + PureProjection prj = null; + int maxZoom; - for (int y = 0; y < answer.GetLength(0) - 1; y++) + GMaps.Instance.AdjustProjection(type, ref prj, out maxZoom); + int zoom = 11; // 12 + if (!area.IsEmpty) { - Console.WriteLine(y); - for (int x = 0; x < answer.GetLength(1) - 1; x++) + try { - double mlat = lat + step * (float)y + Math.Sin(direction * deg2rad) * step * (float)y; - double mlng = lng + step * (float)x + Math.Cos(direction * deg2rad) * step * (float)x; + List<GPoint> tileArea = prj.GetAreaTileList(area, zoom, 0); + //string bigImage = zoom + "-" + type + "-vilnius.png"; + + //Console.WriteLine("Preparing: " + bigImage); + //Console.WriteLine("Zoom: " + zoom); + //Console.WriteLine("Type: " + type.ToString()); + //Console.WriteLine("Area: " + area); + + var types = GMaps.Instance.GetAllLayersOfType(type); - // Console.WriteLine(mlat + " "+mlng); + // current area + GPoint topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, zoom); + GPoint rightButtomPx = prj.FromLatLngToPixel(area.Bottom, area.Right, zoom); + GPoint pxDelta = new GPoint(rightButtomPx.X - topLeftPx.X, rightButtomPx.Y - topLeftPx.Y); - int alt = srtm.getAltitude(mlat, mlng, 20); - answer[x,y] = alt; + int padding = 0; + { + using (Bitmap bmpDestination = new Bitmap(pxDelta.X + padding * 2, pxDelta.Y + padding * 2)) + { + using (Graphics gfx = Graphics.FromImage(bmpDestination)) + { + gfx.CompositingMode = System.Drawing.Drawing2D.CompositingMode.SourceOver; + + // get tiles & combine into one + foreach (var p in tileArea) + { + //Console.WriteLine("Downloading[" + p + "]: " + tileArea.IndexOf(p) + " of " + tileArea.Count); + + foreach (MapType tp in types) + { + Exception ex; + WindowsFormsImage tile = GMaps.Instance.GetImageFrom(tp, p, zoom, out ex) as WindowsFormsImage; + if (tile != null) + { + using (tile) + { + int x = p.X * prj.TileSize.Width - topLeftPx.X + padding; + int y = p.Y * prj.TileSize.Width - topLeftPx.Y + padding; + { + gfx.DrawImage(tile.Img, x, y, prj.TileSize.Width, prj.TileSize.Height); + } + } + } + } + } + } + _terrain = new Bitmap(bmpDestination, 512, 512); + + + GL.BindTexture(TextureTarget.Texture2D, texture); + + BitmapData data = _terrain.LockBits(new System.Drawing.Rectangle(0, 0, _terrain.Width, _terrain.Height), + ImageLockMode.ReadOnly, System.Drawing.Imaging.PixelFormat.Format32bppArgb); + + //Console.WriteLine("w {0} h {1}",data.Width, data.Height); + + GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgba, data.Width, data.Height, 0, + OpenTK.Graphics.OpenGL.PixelFormat.Bgra, PixelType.UnsignedByte, data.Scan0); + + _terrain.UnlockBits(data); + + GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Linear); + GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Linear); + + + } + } } + catch { } } - - return answer; } - int texture = 0; - private System.ComponentModel.IContainer components; + const float rad2deg = (float)(180 / Math.PI); + const float deg2rad = (float)(1.0 / rad2deg); public Vector3 Normal(Vector3 a, Vector3 b, Vector3 c) { @@ -82,17 +152,20 @@ namespace ArdupilotMega.Controls return norm; } - float _angle = 0; - double cameraX, cameraY, cameraZ; // camera coordinates - double lookX, lookY, lookZ; // camera look-at coordinates - protected override void OnPaint(System.Windows.Forms.PaintEventArgs e) { if (this.DesignMode) return; - _angle+=1; + if (area.LocationMiddle.Lat == 0 && area.LocationMiddle.Lng == 0) + return; + + _angle+=1f; + + // area.LocationTopLeft = new PointLatLng(area.LocationTopLeft.Lat + 0.0001,area.LocationTopLeft.Lng); + + //area.Size = new SizeLatLng(0.1, 0.1); try { @@ -101,64 +174,126 @@ namespace ArdupilotMega.Controls } catch { return; } + double heightscale = (step / 90.0) * 4; + float scale = 1.0f; - float radians = (float)(Math.PI*(_angle-90.0f)/180.0f); + float radians = (float)(Math.PI * (rpy.Z * -1) / 180.0f); - int mouseY = (int)(900 * scale); - - // calculate the camera's position - cameraX = lookX + Math.Sin(radians)*mouseY; // multiplying by mouseY makes the - cameraZ = lookZ + Math.Cos(radians)*mouseY; // camera get closer/farther away with mouseY - cameraY = lookY + mouseY / 2.0f; - - // calculate the camera look-at coordinates as the center of the terrain map - lookX = (_terrain.Width * scale) / 2.0f; - lookY = 0 * scale; - lookZ = (_terrain.Height * scale) / 2.0f; + //radians = 0; + float mouseY = (float)(0.1 * scale); + + cameraX = area.LocationMiddle.Lng; // multiplying by mouseY makes the + cameraZ = area.LocationMiddle.Lat; // camera get closer/farther away with mouseY + cameraY = (LocationCenter.Alt < srtm.getAltitude(cameraZ, cameraX, 20)) ? (srtm.getAltitude(cameraZ, cameraX, 20)+ 0.2) * heightscale : LocationCenter.Alt * heightscale;// (srtm.getAltitude(lookZ, lookX, 20) + 100) * heighscale; + + + lookX = area.LocationMiddle.Lng + Math.Sin(radians) * mouseY; ; + lookY = cameraY; + lookZ = area.LocationMiddle.Lat + Math.Cos(radians) * mouseY; ; MakeCurrent(); - GL.ClearColor(Color.Green); + + GL.MatrixMode(MatrixMode.Projection); + + OpenTK.Matrix4 projection = OpenTK.Matrix4.CreatePerspectiveFieldOfView(60 * deg2rad, 1f, 0.00001f, 5000.0f); + GL.LoadMatrix(ref projection); + + Matrix4 modelview = Matrix4.LookAt((float)cameraX, (float)cameraY, (float)cameraZ, (float)lookX, (float)lookY, (float)lookZ, 0,1,0); + GL.MatrixMode(MatrixMode.Modelview); + + // roll + modelview = Matrix4.Mult(modelview,Matrix4.CreateRotationZ (rpy.X * deg2rad)); + // pitch + modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationX(rpy.Y * -deg2rad)); + + GL.LoadMatrix(ref modelview); + + GL.ClearColor(Color.Blue); GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit); - GL.LoadIdentity(); - OpenTK.Graphics.Glu.LookAt(cameraX, cameraY, cameraZ, lookX, lookY, lookZ, 0.0, 1.0, 0.0); + GL.LightModel(LightModelParameter.LightModelAmbient,new float[] {1f,1f,1f,1f}); GL.Enable(EnableCap.Texture2D); - GL.BindTexture(TextureTarget.Texture2D, texture); - - double mlat = -34.73306; - double mlng = 117.8864897; - double step = 0.00083333333333333; + GL.BindTexture(TextureTarget.Texture2D, texture); + /* + GL.Begin(BeginMode.LineStrip); + + GL.Color3(Color.White); + GL.Vertex3(0, 0, 0); + + GL.Vertex3(area.Bottom, 0, area.Left); + + GL.Vertex3(lookX, lookY, lookZ); + + //GL.Vertex3(cameraX, cameraY, cameraZ); + + GL.End(); + */ + System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch(); + + sw.Start(); + + getImage(); + + sw.Stop(); - int increment =50; + Console.WriteLine("img " +sw.ElapsedMilliseconds); - for (int z = 0; z < _terrain.Height - 1; z += increment) + sw.Start(); + + double increment = step * 1; + + double cleanup = area.Bottom % increment; + double cleanup2 = area.Left % increment; + + for (double z = (area.Bottom - cleanup); z < area.Top - step; z += increment) { //Makes OpenGL draw a triangle at every three consecutive vertices GL.Begin(BeginMode.TriangleStrip); - for (int x = 0; x < _terrain.Width - 1; x += increment) + for (double x = (area.Left - cleanup2); x < area.Right - step; x += increment) { + int heightl = srtm.getAltitude(z, area.Right + area.Left - x, 20); + + // Console.WriteLine(x + " " + z); + + GL.Color3(Color.White); - // Console.WriteLine(mlat + step * z +" "+ mlng + step * x); - int heightl = srtm.getAltitude(mlat + step * z, mlng + step * x, 20); + // int heightl = 0; - //GL.Color3(_terrain.GetPixel(x, z)); - GL.TexCoord2((x / (float)_terrain.Width), (z / (float)_terrain.Height)); - GL.Vertex3(x * scale, heightl, z * scale); // _terrain.GetPixel(x, z).R + double scale2 = (Math.Abs(x - area.Left) / area.WidthLng);// / (float)_terrain.Width; + + double scale3 = (Math.Abs(z - area.Bottom) / area.HeightLat);// / (float)_terrain.Height; + + double imgx = 1 - scale2; + double imgy = 1 - scale3; + // GL.Color3(Color.Red); + + //GL.Color3(_terrain.GetPixel(imgx, imgy)); + GL.TexCoord2(imgx,imgy); + GL.Vertex3(x, heightl * heightscale, z); // _terrain.GetPixel(x, z).R try { - heightl = srtm.getAltitude(mlat + step * (z + increment), mlng + step * (x), 20); + heightl = srtm.getAltitude(z + increment, area.Right + area.Left - x, 20); + + //scale2 = (Math.Abs(x - area.Left) / area.WidthLng) * (float)_terrain.Width; + + scale3 = (Math.Abs(((z + increment) - area.Bottom)) / area.HeightLat);// / (float)_terrain.Height; - //GL.Color3(_terrain.GetPixel(x, z + increment)); - GL.TexCoord2((x / (float)_terrain.Width), ((z + increment) / (float)_terrain.Height)); - GL.Vertex3(x * scale, heightl, z + increment * scale); + imgx = 1- scale2; + imgy = 1 - scale3; + // GL.Color3(Color.Green); + //GL.Color3(_terrain.GetPixel(imgx, imgy)); + GL.TexCoord2(imgx,imgy); + GL.Vertex3(x, heightl * heightscale, z + increment); + + // Console.WriteLine(x + " " + (z + step)); } catch { break; } @@ -172,12 +307,16 @@ namespace ArdupilotMega.Controls GL.DepthMask(true); GL.Disable(EnableCap.Blend); - GL.Flush(); - + GL.Flush(); + + + sw.Stop(); + + Console.WriteLine("GL "+sw.ElapsedMilliseconds); this.SwapBuffers(); - // this.Invalidate(); + //this.Invalidate(); } private void InitializeComponent() @@ -198,7 +337,7 @@ namespace ArdupilotMega.Controls { GL.Enable(EnableCap.DepthTest); // GL.Enable(EnableCap.Light0); - // GL.Enable(EnableCap.Lighting); + GL.Enable(EnableCap.Lighting); GL.Enable(EnableCap.ColorMaterial); GL.Enable(EnableCap.Normalize); @@ -213,16 +352,11 @@ namespace ArdupilotMega.Controls private void test_Resize(object sender, EventArgs e) { - GL.Viewport(0, 0, this.Width, this.Height); - GL.MatrixMode(MatrixMode.Projection); - GL.LoadIdentity(); - if (Height == 0) - Height = 1; + MakeCurrent(); - OpenTK.Graphics.Glu.Perspective(54.0f, this.Width / this.Height, 1.0f, 5000.0f); + GL.Viewport(0, 0, this.Width, this.Height); - GL.MatrixMode(MatrixMode.Modelview); - GL.LoadIdentity(); + this.Invalidate(); } } } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index a2f5d34005047b3872cf75af7025ad57f0827460..8f05c1bb53e5d24cc89a746dec09ec66aca4ac88 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -382,6 +382,12 @@ namespace ArdupilotMega.GCSViews updateBindingSource(); //Console.WriteLine(DateTime.Now.Millisecond + " done "); + if (ArdupilotMega.Controls.OpenGLtest.instance != null) + { + ArdupilotMega.Controls.OpenGLtest.instance.rpy = new OpenTK.Vector3(MainV2.cs.roll,MainV2.cs.pitch,MainV2.cs.yaw); + ArdupilotMega.Controls.OpenGLtest.instance.LocationCenter = new PointLatLngAlt(MainV2.cs.lat,MainV2.cs.lng,MainV2.cs.alt,"here"); + } + if (tunning.AddMilliseconds(50) < DateTime.Now && CB_tuning.Checked == true) { diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index ec49653fae3513db39584db5ec517242ece286dd..080771bc29b60743f856652a60e72c57d20b0e19 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -699,7 +699,7 @@ namespace ArdupilotMega mavlink_param_value_t par = buffer.ByteArrayToStructure<mavlink_param_value_t>(6); // set new target - param_total = (par.param_count - 1); + param_total = (par.param_count); string paramID = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id); @@ -718,7 +718,7 @@ namespace ArdupilotMega continue; } - log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (par.param_count - 2) + " name: " + paramID); + log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (par.param_count) + " name: " + paramID); modifyParamForDisplay(true, paramID, ref par.param_value); param[paramID] = (par.param_value); @@ -730,7 +730,7 @@ namespace ArdupilotMega this.frmProgressReporter.UpdateProgressAndStatus((got.Count * 100) / param_total, "Got param " + paramID); // we have them all - lets escape eq total = 176 index = 0-175 - if (par.param_index == (param_total - 1)) + if (par.param_index == (param_total -1)) break; } else @@ -1524,7 +1524,7 @@ namespace ArdupilotMega /// </summary> /// <param name="datin">packet byte array</param> /// <returns>struct of data</returns> - public object DebugPacket(byte[] datin, ref string text, bool PrintToConsole) + public object DebugPacket(byte[] datin, ref string text, bool PrintToConsole, string delimeter = " ") { string textoutput; try @@ -1538,7 +1538,7 @@ namespace ArdupilotMega byte compid = datin[4]; byte messid = datin[5]; - textoutput = string.Format("{0:X} {1:X} {2:X} {3:X} {4:X} {5:X} ", header, length, seq, sysid, compid, messid); + textoutput = string.Format("{0:X}{6}{1:X}{6}{2:X}{6}{3:X}{6}{4:X}{6}{5:X}{6}", header, length, seq, sysid, compid, messid, delimeter); object data = Activator.CreateInstance(MAVLINK_MESSAGE_INFO[messid]); @@ -1549,7 +1549,7 @@ namespace ArdupilotMega if (PrintToConsole) { - textoutput = textoutput + test.Name + " "; + textoutput = textoutput + test.Name + delimeter; foreach (var field in test.GetFields()) { @@ -1559,7 +1559,7 @@ namespace ArdupilotMega if (field.FieldType.IsArray) { - textoutput = textoutput + field.Name + "="; + textoutput = textoutput + field.Name + delimeter; byte[] crap = (byte[])fieldValue; foreach (byte fiel in crap) { @@ -1572,14 +1572,14 @@ namespace ArdupilotMega textoutput = textoutput + (char)fiel; } } - textoutput = textoutput + " "; + textoutput = textoutput + delimeter; } else { - textoutput = textoutput + field.Name + "=" + fieldValue.ToString() + " "; + textoutput = textoutput + field.Name + delimeter + fieldValue.ToString() + delimeter; } } - textoutput = textoutput + " Len:" + datin.Length + "\r\n"; + textoutput = textoutput + delimeter + "Len" + delimeter + datin.Length + "\r\n"; if (PrintToConsole) Console.Write(textoutput); diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index a83f94353e31e1dc39f46bb3fec9087e2f856648..d22c652b10079e2444353225c07b06c08c9e2781 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -712,6 +712,8 @@ namespace ArdupilotMega log.Warn(ex.ToString()); try { + _connectionControl.IsConnected(false); + UpdateConnectIcon(); comPort.Close(); } catch { } diff --git a/Tools/ArdupilotMegaPlanner/MavlinkLog.Designer.cs b/Tools/ArdupilotMegaPlanner/MavlinkLog.Designer.cs index 0a77c3dfce3f8356cf7530d745530f7f5c5bc7b7..256cde8672616f093253073bcafa4a6cb2a31507 100644 --- a/Tools/ArdupilotMegaPlanner/MavlinkLog.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/MavlinkLog.Designer.cs @@ -34,6 +34,7 @@ this.BUT_humanreadable = new ArdupilotMega.Controls.MyButton(); this.BUT_graphmavlog = new ArdupilotMega.Controls.MyButton(); this.zg1 = new ZedGraph.ZedGraphControl(); + this.BUT_convertcsv = new ArdupilotMega.Controls.MyButton(); this.SuspendLayout(); // // BUT_redokml @@ -74,10 +75,18 @@ this.zg1.ScrollMinY = 0D; this.zg1.ScrollMinY2 = 0D; // + // BUT_convertcsv + // + resources.ApplyResources(this.BUT_convertcsv, "BUT_convertcsv"); + this.BUT_convertcsv.Name = "BUT_convertcsv"; + this.BUT_convertcsv.UseVisualStyleBackColor = true; + this.BUT_convertcsv.Click += new System.EventHandler(this.BUT_convertcsv_Click); + // // MavlinkLog // resources.ApplyResources(this, "$this"); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; + this.Controls.Add(this.BUT_convertcsv); this.Controls.Add(this.zg1); this.Controls.Add(this.BUT_graphmavlog); this.Controls.Add(this.BUT_humanreadable); @@ -96,5 +105,6 @@ private ArdupilotMega.Controls.MyButton BUT_humanreadable; private ArdupilotMega.Controls.MyButton BUT_graphmavlog; private ZedGraph.ZedGraphControl zg1; + private Controls.MyButton BUT_convertcsv; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs index f5422532e9dc2d884498ccdbd99a0c23b90da62e..a2fa1fc7cf53f5f533cb8d65c601af7f216b72f4 100644 --- a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs +++ b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs @@ -1154,5 +1154,58 @@ namespace ArdupilotMega return selectform; } + + private void BUT_convertcsv_Click(object sender, EventArgs e) + { + OpenFileDialog openFileDialog1 = new OpenFileDialog(); + openFileDialog1.Filter = "*.tlog|*.tlog"; + openFileDialog1.FilterIndex = 2; + openFileDialog1.RestoreDirectory = true; + openFileDialog1.Multiselect = true; + try + { + openFileDialog1.InitialDirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar; + } + catch { } // incase dir doesnt exist + + if (openFileDialog1.ShowDialog() == DialogResult.OK) + { + foreach (string logfile in openFileDialog1.FileNames) + { + + MAVLink mine = new MAVLink(); + mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); + mine.logreadmode = true; + + mine.packets.Initialize(); // clear + + StreamWriter sw = new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".csv"); + + while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) + { + // bar moves to 100 % in this step + progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 1.0f); + + progressBar1.Refresh(); + //Application.DoEvents(); + + byte[] packet = mine.readPacket(); + string text = ""; + mine.DebugPacket(packet, ref text,true,","); + + sw.Write(mine.lastlogread + "," + text); + } + + sw.Close(); + + progressBar1.Value = 100; + + mine.logreadmode = false; + mine.logplaybackfile.Close(); + mine.logplaybackfile = null; + + } + } + } } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/MavlinkLog.resx b/Tools/ArdupilotMegaPlanner/MavlinkLog.resx index ec4f313eb2a2ab97a40ae6a344603890235fe1f4..11b80999a6d6407c54fc5a39a292e3da337f7377 100644 --- a/Tools/ArdupilotMegaPlanner/MavlinkLog.resx +++ b/Tools/ArdupilotMegaPlanner/MavlinkLog.resx @@ -123,7 +123,7 @@ </data> <assembly alias="System.Drawing" name="System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a" /> <data name="BUT_redokml.Location" type="System.Drawing.Point, System.Drawing"> - <value>143, 12</value> + <value>93, 12</value> </data> <data name="BUT_redokml.Size" type="System.Drawing.Size, System.Drawing"> <value>116, 23</value> @@ -139,13 +139,13 @@ <value>BUT_redokml</value> </data> <data name=">>BUT_redokml.Type" xml:space="preserve"> - <value>ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null</value> + <value>ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.1.4515.21878, Culture=neutral, PublicKeyToken=null</value> </data> <data name=">>BUT_redokml.Parent" xml:space="preserve"> <value>$this</value> </data> <data name=">>BUT_redokml.ZOrder" xml:space="preserve"> - <value>4</value> + <value>5</value> </data> <data name="progressBar1.Anchor" type="System.Windows.Forms.AnchorStyles, System.Windows.Forms"> <value>Top, Left, Right</value> @@ -169,7 +169,7 @@ <value>$this</value> </data> <data name=">>progressBar1.ZOrder" xml:space="preserve"> - <value>3</value> + <value>4</value> </data> <data name="BUT_humanreadable.Anchor" type="System.Windows.Forms.AnchorStyles, System.Windows.Forms"> <value>Top</value> @@ -178,7 +178,7 @@ <value>NoControl</value> </data> <data name="BUT_humanreadable.Location" type="System.Drawing.Point, System.Drawing"> - <value>265, 12</value> + <value>215, 12</value> </data> <data name="BUT_humanreadable.Size" type="System.Drawing.Size, System.Drawing"> <value>116, 23</value> @@ -193,13 +193,13 @@ <value>BUT_humanreadable</value> </data> <data name=">>BUT_humanreadable.Type" xml:space="preserve"> - <value>ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null</value> + <value>ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.1.4515.21878, Culture=neutral, PublicKeyToken=null</value> </data> <data name=">>BUT_humanreadable.Parent" xml:space="preserve"> <value>$this</value> </data> <data name=">>BUT_humanreadable.ZOrder" xml:space="preserve"> - <value>2</value> + <value>3</value> </data> <data name="BUT_graphmavlog.Anchor" type="System.Windows.Forms.AnchorStyles, System.Windows.Forms"> <value>Top</value> @@ -208,7 +208,7 @@ <value>NoControl</value> </data> <data name="BUT_graphmavlog.Location" type="System.Drawing.Point, System.Drawing"> - <value>387, 12</value> + <value>459, 12</value> </data> <data name="BUT_graphmavlog.Size" type="System.Drawing.Size, System.Drawing"> <value>116, 23</value> @@ -223,13 +223,13 @@ <value>BUT_graphmavlog</value> </data> <data name=">>BUT_graphmavlog.Type" xml:space="preserve"> - <value>ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null</value> + <value>ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.1.4515.21878, Culture=neutral, PublicKeyToken=null</value> </data> <data name=">>BUT_graphmavlog.Parent" xml:space="preserve"> <value>$this</value> </data> <data name=">>BUT_graphmavlog.ZOrder" xml:space="preserve"> - <value>1</value> + <value>2</value> </data> <data name="zg1.Anchor" type="System.Windows.Forms.AnchorStyles, System.Windows.Forms"> <value>Top, Bottom, Left, Right</value> @@ -253,6 +253,36 @@ <value>$this</value> </data> <data name=">>zg1.ZOrder" xml:space="preserve"> + <value>1</value> + </data> + <data name="BUT_convertcsv.Anchor" type="System.Windows.Forms.AnchorStyles, System.Windows.Forms"> + <value>Top</value> + </data> + <data name="BUT_convertcsv.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms"> + <value>NoControl</value> + </data> + <data name="BUT_convertcsv.Location" type="System.Drawing.Point, System.Drawing"> + <value>337, 12</value> + </data> + <data name="BUT_convertcsv.Size" type="System.Drawing.Size, System.Drawing"> + <value>116, 23</value> + </data> + <data name="BUT_convertcsv.TabIndex" type="System.Int32, mscorlib"> + <value>13</value> + </data> + <data name="BUT_convertcsv.Text" xml:space="preserve"> + <value>Convert to CSV</value> + </data> + <data name=">>BUT_convertcsv.Name" xml:space="preserve"> + <value>BUT_convertcsv</value> + </data> + <data name=">>BUT_convertcsv.Type" xml:space="preserve"> + <value>ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.1.4515.21878, Culture=neutral, PublicKeyToken=null</value> + </data> + <data name=">>BUT_convertcsv.Parent" xml:space="preserve"> + <value>$this</value> + </data> + <data name=">>BUT_convertcsv.ZOrder" xml:space="preserve"> <value>0</value> </data> <metadata name="$this.Localizable" type="System.Boolean, mscorlib, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089"> diff --git a/Tools/ArdupilotMegaPlanner/Program.cs b/Tools/ArdupilotMegaPlanner/Program.cs index f565d566a8cf92a08172c4a5ee80d62cbb100fe7..d2206e233411fa811e3e47d5405ef24dc759d25b 100644 --- a/Tools/ArdupilotMegaPlanner/Program.cs +++ b/Tools/ArdupilotMegaPlanner/Program.cs @@ -41,6 +41,10 @@ namespace ArdupilotMega //Common.linearRegression(); + //Console.WriteLine(srtm.getAltitude(-35.115676879882812, 117.94178754638671,20)); + + //return; + if (System.Diagnostics.Debugger.IsAttached) { // testing diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index baff7373f991ba3800cf3c63426ce19024d43bd7..c517b469646edaafcb4a1e09af72eab51a2ac10e 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.1.*")] -[assembly: AssemblyFileVersion("1.1.84")] +[assembly: AssemblyFileVersion("1.1.85")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/georefimage.cs b/Tools/ArdupilotMegaPlanner/georefimage.cs index 88dc1ccd81395aac88e169afbb1f8fa1ce38d723..363fd7679f2613dd42bd332098bb72ab82261c02 100644 --- a/Tools/ArdupilotMegaPlanner/georefimage.cs +++ b/Tools/ArdupilotMegaPlanner/georefimage.cs @@ -53,6 +53,8 @@ namespace ArdupilotMega Hashtable timecoordcache = new Hashtable(); Hashtable imagetotime = new Hashtable(); + List<string[]> logcache = new List<string[]>(); + DateTime getPhotoTime(string fn) { DateTime dtaken = DateTime.MinValue; @@ -120,6 +122,9 @@ namespace ArdupilotMega List<string[]> readLog(string fn) { + if (logcache.Count > 0) + return logcache; + List<string[]> list = new List<string[]>(); if (fn.ToLower().EndsWith("tlog")) @@ -146,7 +151,7 @@ namespace ArdupilotMega // line "GPS: 82686250, 1, 8, -34.1406480, 118.5441900, 0.0000, 309.1900, 315.9500, 0.0000, 279.1200" string - string[] vals = new string[] { "GPS", (cs.datetime - new DateTime(cs.datetime.Year,cs.datetime.Month,cs.datetime.Day,0,0,0,DateTimeKind.Local)).TotalMilliseconds.ToString(), "1", + string[] vals = new string[] { "GPS", (cs.datetime.ToUniversalTime() - new DateTime(cs.datetime.Year,cs.datetime.Month,cs.datetime.Day,0,0,0,DateTimeKind.Utc)).TotalMilliseconds.ToString(), "1", cs.satcount.ToString(),cs.lat.ToString(),cs.lng.ToString(),"0.0",cs.alt.ToString(),cs.alt.ToString(),cs.groundspeed.ToString(),cs.yaw.ToString()}; if (oldvalues.Length > 2 && oldvalues[latpos] == vals[latpos] @@ -164,6 +169,8 @@ namespace ArdupilotMega mine.logplaybackfile.Close(); + logcache = list; + return list; } @@ -191,6 +198,8 @@ namespace ArdupilotMega sr.Close(); sr.Dispose(); + logcache = list; + return list; } @@ -452,6 +461,7 @@ namespace ArdupilotMega this.TXT_logfile.Size = new System.Drawing.Size(317, 20); this.TXT_logfile.TabIndex = 2; this.TXT_logfile.Text = "C:\\Users\\hog\\Pictures\\farm 1-10-2011\\100SSCAM\\2011-10-01 11-48 1.log"; + this.TXT_logfile.TextChanged += new System.EventHandler(this.TXT_logfile_TextChanged); // // TXT_jpgdir // @@ -681,6 +691,8 @@ namespace ArdupilotMega private void BUT_browselog_Click(object sender, EventArgs e) { + logcache.Clear(); + openFileDialog1.Filter = "Logs|*.log;*.tlog"; openFileDialog1.ShowDialog(); @@ -830,6 +842,11 @@ namespace ArdupilotMega { System.Diagnostics.Process.Start(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "m3u" + Path.DirectorySeparatorChar + "GeoRefnetworklink.kml"); } + + private void TXT_logfile_TextChanged(object sender, EventArgs e) + { + logcache.Clear(); + } } public class Rational diff --git a/Tools/ArdupilotMegaPlanner/srtm.cs b/Tools/ArdupilotMegaPlanner/srtm.cs index 73d25f6ec09d26ad4f5ab12b141bd39ab00e0a8f..a17ca8274802bc1684ef5c52f1b26650ed95bd8d 100644 --- a/Tools/ArdupilotMegaPlanner/srtm.cs +++ b/Tools/ArdupilotMegaPlanner/srtm.cs @@ -13,7 +13,7 @@ namespace ArdupilotMega { class srtm { - public static string datadirectory; + public static string datadirectory = "./srtm/"; static List<string> allhgts = new List<string>(); @@ -23,23 +23,20 @@ namespace ArdupilotMega static List<string> queue = new List<string>(); - static Hashtable altcache = new Hashtable(); + static Hashtable fnamecache = new Hashtable(); + + static Hashtable filecache = new Hashtable(); public static int getAltitude(double lat, double lng, double zoom) { - if (!Directory.Exists(datadirectory)) - Directory.CreateDirectory(datadirectory); - short alt = 0; - lat += 0.00083333333333333; - //lng += 0.0008; - - if (altcache[lat * lng] != null) - return (int)(short)altcache[lat * lng]; + lat += 1 / 1199.0; + //lng -= 1 / 1201f; - if (altcache.Count > 10000) - altcache = new Hashtable(); + // lat -35.115676879882812 double + // lng 117.94178754638671 double + // alt 70 short int x = (int)Math.Floor(lng); int y = (int)Math.Floor(lat); @@ -56,19 +53,24 @@ namespace ArdupilotMega else ew = "W"; - string filename = ns + Math.Abs(y).ToString("00") + ew + Math.Abs(x).ToString("000") + ".hgt"; + if (fnamecache[y] == null) + fnamecache[y] = Math.Abs(y).ToString("00"); + if (fnamecache[1000 + x] == null) + fnamecache[1000 + x] = Math.Abs(x).ToString("000"); - string filename2 = "srtm_" + Math.Round((lng + 2.5 + 180) / 5, 0).ToString("00") + "_" + Math.Round((60 - lat + 2.5) / 5, 0).ToString("00") + ".asc"; + string filename = ns + fnamecache[y] + ew + fnamecache[1000 + x] + ".hgt"; try { - if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename)) + if (filecache.ContainsKey(datadirectory + Path.DirectorySeparatorChar + filename) || File.Exists(datadirectory + Path.DirectorySeparatorChar + filename)) { // srtm hgt files - FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open, FileAccess.Read, FileShare.Read); + //FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open, FileAccess.Read, FileShare.Read); - float posx = 0; - float row = 0; + MemoryStream fs = readFile(datadirectory + Path.DirectorySeparatorChar + filename); + + int posx = 0; + int row = 0; if (fs.Length <= (1201 * 1201 * 2)) { @@ -95,23 +97,20 @@ namespace ArdupilotMega fs.Seek((int)(row + posx), SeekOrigin.Begin); fs.Read(data, 0, data.Length); - fs.Close(); - fs.Dispose(); - - Array.Reverse(data); + //fs.Close(); - alt = BitConverter.ToInt16(data, 0); + //Array.Reverse(data); - altcache[lat * lng] = alt; + alt = (short)((data[0] << 8) + data[1]) ;//BitConverter.ToInt16(data, 0); return alt; } - else if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename2)) - { - // this is way to slow - and cacheing it will chew memory 6001 * 6001 * 4 = 144048004 bytes - FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename2, FileMode.Open, FileAccess.Read); - StreamReader sr = new StreamReader(fs); + string filename2 = "srtm_" + Math.Round((lng + 2.5 + 180) / 5, 0).ToString("00") + "_" + Math.Round((60 - lat + 2.5) / 5, 0).ToString("00") + ".asc"; + + if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename2)) + { + StreamReader sr = new StreamReader(readFile(datadirectory + Path.DirectorySeparatorChar + filename2)); int nox = 0; int noy = 0; @@ -191,12 +190,17 @@ namespace ArdupilotMega } + //sr.Close(); + return alt; } else // get something { if (zoom >= 15) { + if (!Directory.Exists(datadirectory)) + Directory.CreateDirectory(datadirectory); + if (requestThread == null) { Console.WriteLine("Getting " + filename); @@ -227,6 +231,27 @@ namespace ArdupilotMega return alt; } + static MemoryStream readFile(string filename) + { + if (filecache.ContainsKey(filename)) + { + return (MemoryStream)filecache[filename]; + } + else + { + FileStream fs = new FileStream(filename, FileMode.Open, FileAccess.Read); + + byte[] file = new byte[fs.Length]; + fs.Read(file, 0, (int)fs.Length); + + filecache[filename] = new MemoryStream(file); + + fs.Close(); + + return (MemoryStream)filecache[filename]; + } + } + static void requestRunner() { while (true) @@ -252,7 +277,7 @@ namespace ArdupilotMega } } catch { } - Thread.Sleep(100); + Thread.Sleep(500); } } @@ -270,7 +295,9 @@ namespace ArdupilotMega foreach (string item in list) { - List<string> hgtfiles = getListing(item); + List<string> hgtfiles = new List<string>(); + + hgtfiles = getListing(item); foreach (string hgt in hgtfiles) { @@ -321,10 +348,30 @@ namespace ArdupilotMega static List<string> getListing(string url) { + string name = new Uri(url).AbsolutePath; + + name = Path.GetFileName(name.TrimEnd('/')); + List<string> list = new List<string>(); + if (File.Exists(datadirectory + Path.DirectorySeparatorChar + name)) + { + StreamReader sr = new StreamReader(datadirectory + Path.DirectorySeparatorChar + name); + + while (!sr.EndOfStream) + { + list.Add(sr.ReadLine()); + } + + sr.Close(); + + return list; + } + + try { + StreamWriter sw = new StreamWriter(datadirectory + Path.DirectorySeparatorChar + name); WebRequest req = HttpWebRequest.Create(url); @@ -349,6 +396,13 @@ namespace ArdupilotMega } } + + list.ForEach(x => + { + sw.WriteLine((string)x); + }); + + sw.Close(); } catch { } diff --git a/Tools/ArdupilotMegaPlanner/temp.cs b/Tools/ArdupilotMegaPlanner/temp.cs index 18c528c433ccc5062c7893ba4254b919e028bc00..13385a39cd208e2d90048d80ea4c7ebb063e1fa4 100644 --- a/Tools/ArdupilotMegaPlanner/temp.cs +++ b/Tools/ArdupilotMegaPlanner/temp.cs @@ -32,6 +32,15 @@ namespace ArdupilotMega public temp() { InitializeComponent(); + + if (System.Diagnostics.Debugger.IsAttached) { + + ArdupilotMega.Controls.OpenGLtest ogl = new Controls.OpenGLtest(); + + this.Controls.Add(ogl); + + ogl.Dock = DockStyle.Fill; + } } private void temp_Load(object sender, EventArgs e)