From d1139761c0908afa35393054b1ea353c1ce1314f Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sun, 13 May 2012 18:56:42 +0800 Subject: [PATCH] APM Planner 1.1.85 work on opengltest fix last missing param fix com port selection on connecting to bad comport add convert to csv. entire log add log caching to image georef. for tlog and logs add srtm data caching --- .../ArdupilotMegaPlanner/ArdupilotMega.csproj | 4 - .../ArdupilotMega.csproj.user | 2 +- Tools/ArdupilotMegaPlanner/Common.cs | 5 + .../Controls/OpenGLtest.cs | 324 +++++++++++++----- .../GCSViews/FlightData.cs | 6 + Tools/ArdupilotMegaPlanner/MAVLink.cs | 20 +- Tools/ArdupilotMegaPlanner/MainV2.cs | 2 + .../MavlinkLog.Designer.cs | 10 + Tools/ArdupilotMegaPlanner/MavlinkLog.cs | 53 +++ Tools/ArdupilotMegaPlanner/MavlinkLog.resx | 50 ++- Tools/ArdupilotMegaPlanner/Program.cs | 4 + .../Properties/AssemblyInfo.cs | 2 +- Tools/ArdupilotMegaPlanner/georefimage.cs | 19 +- Tools/ArdupilotMegaPlanner/srtm.cs | 116 +++++-- Tools/ArdupilotMegaPlanner/temp.cs | 9 + 15 files changed, 473 insertions(+), 153 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index d229a5f1be..215231803a 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -174,10 +174,6 @@ False Lib\OpenTK.dll - - False - ..\..\..\..\..\Desktop\DIYDrones\opentk\trunk\Binaries\OpenTK\Debug\OpenTK.Compatibility.dll - False diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj.user b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj.user index 730cb9c526..e53d971127 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj.user +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj.user @@ -11,6 +11,6 @@ - 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\ + 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\ \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Common.cs b/Tools/ArdupilotMegaPlanner/Common.cs index ebc4326f59..680a803876 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; + } + /// /// Calc Distance in M /// diff --git a/Tools/ArdupilotMegaPlanner/Controls/OpenGLtest.cs b/Tools/ArdupilotMegaPlanner/Controls/OpenGLtest.cs index 9b5b9f4098..b3baf42517 100644 --- a/Tools/ArdupilotMegaPlanner/Controls/OpenGLtest.cs +++ b/Tools/ArdupilotMegaPlanner/Controls/OpenGLtest.cs @@ -6,75 +6,145 @@ 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; + + float _angle = 0; + double cameraX, cameraY, cameraZ; // camera coordinates + double lookX, lookY, lookZ; // camera look-at coordinates + + double step = 1 / 1200.0; + + 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 { + + if (area.LocationMiddle.Lat == value.Lat && area.LocationMiddle.Lng == value.Lng) + return; + + if (value.Lat == 0 && value.Lng == 0) + return; + + _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(); + } + } + + public Vector3 rpy = new Vector3(); public OpenGLtest() { + instance = this; + InitializeComponent(); - try - { - _terrain = new Bitmap(@"C:\Users\hog\Pictures\Denmark\[Group 1]-P1020169_P1020174-6 images.jpg"); - } - catch { } - - _terrain = new Bitmap(_terrain, 512, 512); - - GL.GenTextures(1, out texture); - 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); + void getImage() + { + MapType type = MapType.GoogleSatellite; + PureProjection prj = null; + int maxZoom; - //Console.WriteLine("w {0} h {1}",data.Width, data.Height); + GMaps.Instance.AdjustProjection(type, ref prj, out maxZoom); + int zoom = 11; // 12 + if (!area.IsEmpty) + { + try + { + List tileArea = prj.GetAreaTileList(area, zoom, 0); + //string bigImage = zoom + "-" + type + "-vilnius.png"; - GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgba, data.Width, data.Height, 0, - OpenTK.Graphics.OpenGL.PixelFormat.Bgra, PixelType.UnsignedByte, data.Scan0); + //Console.WriteLine("Preparing: " + bigImage); + //Console.WriteLine("Zoom: " + zoom); + //Console.WriteLine("Type: " + type.ToString()); + //Console.WriteLine("Area: " + area); - _terrain.UnlockBits(data); + var types = GMaps.Instance.GetAllLayersOfType(type); - GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Linear); - GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Linear); + // 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 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 { } + } } const float rad2deg = (float)(180 / Math.PI); const float deg2rad = (float)(1.0 / rad2deg); - int[,] getElevationData(double lat, double lng, double direction) - { - int[,] answer = new int[400,400]; - - double step = 0.00083333333333333; - - for (int y = 0; y < answer.GetLength(0) - 1; y++) - { - Console.WriteLine(y); - for (int x = 0; x < answer.GetLength(1) - 1; x++) - { - 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; - - // Console.WriteLine(mlat + " "+mlng); - - int alt = srtm.getAltitude(mlat, mlng, 20); - answer[x,y] = alt; - } - } - - return answer; - } - - int texture = 0; - private System.ComponentModel.IContainer components; - public Vector3 Normal(Vector3 a, Vector3 b, Vector3 c) { var dir = Vector3.Cross(b - a, c - a); @@ -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); - int increment =50; + GL.Color3(Color.White); + GL.Vertex3(0, 0, 0); - for (int z = 0; z < _terrain.Height - 1; z += increment) + 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(); + + Console.WriteLine("img " +sw.ElapsedMilliseconds); + + 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(mlat + step * z +" "+ mlng + step * x); + // Console.WriteLine(x + " " + z); - int heightl = srtm.getAltitude(mlat + step * z, mlng + step * x, 20); + GL.Color3(Color.White); - //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 + + // int heightl = 0; + + 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); - //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); + //scale2 = (Math.Abs(x - area.Left) / area.WidthLng) * (float)_terrain.Width; + + scale3 = (Math.Abs(((z + increment) - area.Bottom)) / area.HeightLat);// / (float)_terrain.Height; + + 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) { + MakeCurrent(); + GL.Viewport(0, 0, this.Width, this.Height); - GL.MatrixMode(MatrixMode.Projection); - GL.LoadIdentity(); - if (Height == 0) - Height = 1; - OpenTK.Graphics.Glu.Perspective(54.0f, this.Width / this.Height, 1.0f, 5000.0f); - - GL.MatrixMode(MatrixMode.Modelview); - GL.LoadIdentity(); + this.Invalidate(); } } } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index a2f5d34005..8f05c1bb53 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 ec49653fae..080771bc29 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -699,7 +699,7 @@ namespace ArdupilotMega mavlink_param_value_t par = buffer.ByteArrayToStructure(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 /// /// packet byte array /// struct of data - 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 a83f94353e..d22c652b10 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 0a77c3dfce..256cde8672 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 f5422532e9..a2fa1fc7cf 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 ec4f313eb2..11b80999a6 100644 --- a/Tools/ArdupilotMegaPlanner/MavlinkLog.resx +++ b/Tools/ArdupilotMegaPlanner/MavlinkLog.resx @@ -123,7 +123,7 @@ - 143, 12 + 93, 12 116, 23 @@ -139,13 +139,13 @@ BUT_redokml - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.1.4515.21878, Culture=neutral, PublicKeyToken=null $this - 4 + 5 Top, Left, Right @@ -169,7 +169,7 @@ $this - 3 + 4 Top @@ -178,7 +178,7 @@ NoControl - 265, 12 + 215, 12 116, 23 @@ -193,13 +193,13 @@ BUT_humanreadable - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.1.4515.21878, Culture=neutral, PublicKeyToken=null $this - 2 + 3 Top @@ -208,7 +208,7 @@ NoControl - 387, 12 + 459, 12 116, 23 @@ -223,13 +223,13 @@ BUT_graphmavlog - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.1.4515.21878, Culture=neutral, PublicKeyToken=null $this - 1 + 2 Top, Bottom, Left, Right @@ -253,6 +253,36 @@ $this + 1 + + + Top + + + NoControl + + + 337, 12 + + + 116, 23 + + + 13 + + + Convert to CSV + + + BUT_convertcsv + + + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.1.4515.21878, Culture=neutral, PublicKeyToken=null + + + $this + + 0 diff --git a/Tools/ArdupilotMegaPlanner/Program.cs b/Tools/ArdupilotMegaPlanner/Program.cs index f565d566a8..d2206e2334 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 baff7373f9..c517b46964 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 88dc1ccd81..363fd7679f 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 logcache = new List(); + DateTime getPhotoTime(string fn) { DateTime dtaken = DateTime.MinValue; @@ -120,6 +122,9 @@ namespace ArdupilotMega List readLog(string fn) { + if (logcache.Count > 0) + return logcache; + List list = new List(); 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 73d25f6ec0..a17ca82748 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 allhgts = new List(); @@ -23,23 +23,20 @@ namespace ArdupilotMega static List queue = new List(); - 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; + lat += 1 / 1199.0; + //lng -= 1 / 1201f; - if (altcache[lat * lng] != null) - return (int)(short)altcache[lat * lng]; - - 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(); + //fs.Close(); - Array.Reverse(data); + //Array.Reverse(data); - alt = BitConverter.ToInt16(data, 0); - - 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 hgtfiles = getListing(item); + List hgtfiles = new List(); + + hgtfiles = getListing(item); foreach (string hgt in hgtfiles) { @@ -321,10 +348,30 @@ namespace ArdupilotMega static List getListing(string url) { + string name = new Uri(url).AbsolutePath; + + name = Path.GetFileName(name.TrimEnd('/')); + List list = new List(); + 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 18c528c433..13385a39cd 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)