From d1139761c0908afa35393054b1ea353c1ce1314f Mon Sep 17 00:00:00 2001 From: Michael Oborne <mich146@hotmail.com> 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 | 310 +++++++++++++----- .../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, 466 insertions(+), 146 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index d229a5f1b..215231803 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 730cb9c52..e53d97112 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 ebc4326f5..680a80387 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 9b5b9f409..b3baf4251 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 a2f5d3400..8f05c1bb5 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 ec49653fa..080771bc2 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 a83f94353..d22c652b1 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 0a77c3dfc..256cde867 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 f5422532e..a2fa1fc7c 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 ec4f313eb..11b80999a 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 f565d566a..d2206e233 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 baff7373f..c517b4696 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 88dc1ccd8..363fd7679 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 73d25f6ec..a17ca8274 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 18c528c43..13385a39c 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) -- GitLab