From 086412ecc3736ea0029a936537c4bd6f6655bd4e Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Wed, 12 Oct 2011 21:37:39 +0800 Subject: [PATCH] APM Planner 1.0.82 TCP connection joystick tweaks SIL Prep --- .../ArdupilotMegaPlanner/ArdupilotMega.csproj | 1 + Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs | 222 ++++++++++++++++++ .../GCSViews/Simulation.cs | 7 +- Tools/ArdupilotMegaPlanner/JoystickSetup.cs | 2 +- Tools/ArdupilotMegaPlanner/MainV2.cs | 7 + .../Properties/AssemblyInfo.cs | 2 +- .../bin/Release/.gitignore | 3 +- .../Release/ArdupilotMegaPlanner.application | 2 +- 8 files changed, 240 insertions(+), 6 deletions(-) create mode 100644 Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index 41699710c2..4db94cd354 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -188,6 +188,7 @@ Component + diff --git a/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs b/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs new file mode 100644 index 0000000000..0911ebefb6 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs @@ -0,0 +1,222 @@ +using System; +using System.Collections.Generic; +using System.ComponentModel; +using System.Text; +using System.IO.Ports; +using System.Threading; +using System.Net; // dns, ip address +using System.Net.Sockets; // tcplistner + +namespace System.IO.Ports +{ + public class TcpSerial : ArdupilotMega.ICommsSerial + { + TcpClient client = new TcpClient(); + IPEndPoint RemoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0); + byte[] rbuffer = new byte[0]; + int rbufferread = 0; + + public int WriteBufferSize { get; set; } + public int WriteTimeout { get; set; } + public int ReceivedBytesThreshold { get; set; } + public bool RtsEnable { get; set; } + + ~TcpSerial() + { + this.Close(); + client = null; + } + + public TcpSerial() + { + //System.Threading.Thread.CurrentThread.CurrentUICulture = new System.Globalization.CultureInfo("en-US"); + //System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-US"); + + Port = "5760"; + } + + public string Port { get; set; } + + public int ReadTimeout + { + get;// { return client.ReceiveTimeout; } + set;// { client.ReceiveTimeout = value; } + } + + public int ReadBufferSize {get;set;} + + public int BaudRate { get; set; } + public StopBits StopBits { get; set; } + public Parity Parity { get; set; } + public int DataBits { get; set; } + + public string PortName { get; set; } + + public int BytesToRead + { + get { return client.Available + rbuffer.Length - rbufferread; } + } + + public bool IsOpen { get { return client.Client.Connected; } } + + public bool DtrEnable + { + get; + set; + } + + public void Open() + { + if (client.Client.Connected) + { + Console.WriteLine("tcpserial socket already open"); + return; + } + + string dest = Port; + string host = "127.0.0.1"; + ArdupilotMega.Common.InputBox("remote host", "Enter host name/ip (ensure remote end is already started)", ref host); + ArdupilotMega.Common.InputBox("remote Port", "Enter remote port", ref dest); + Port = dest; + + client = new TcpClient(host, int.Parse(Port)); + + client.NoDelay = true; + client.Client.NoDelay = true; + + return; + } + + void VerifyConnected() + { + if (client == null || !IsOpen) + { + throw new Exception("The socket/serialproxy is closed"); + } + } + + public int Read(byte[] readto,int offset,int length) + { + VerifyConnected(); + try + { + if (length < 1) { return 0; } + + return client.Client.Receive(readto, offset, length, SocketFlags.None); + } + catch { throw new Exception("Socket Closed"); } + } + + public int ReadByte() + { + VerifyConnected(); + int count = 0; + while (this.BytesToRead == 0) + { + System.Threading.Thread.Sleep(1); + if (count > ReadTimeout) + throw new Exception("NetSerial Timeout on read"); + count++; + } + byte[] buffer = new byte[1]; + Read(buffer, 0, 1); + return buffer[0]; + } + + public int ReadChar() + { + return ReadByte(); + } + + public string ReadExisting() + { + VerifyConnected(); + byte[] data = new byte[client.Available]; + if (data.Length > 0) + Read(data, 0, data.Length); + + string line = Encoding.ASCII.GetString(data, 0, data.Length); + + return line; + } + + public void WriteLine(string line) + { + VerifyConnected(); + line = line + "\n"; + Write(line); + } + + public void Write(string line) + { + VerifyConnected(); + byte[] data = new System.Text.ASCIIEncoding().GetBytes(line); + Write(data, 0, data.Length); + } + + public void Write(byte[] write, int offset, int length) + { + VerifyConnected(); + try + { + client.Client.Send(write, length,SocketFlags.None); + } + catch { }//throw new Exception("Comport / Socket Closed"); } + } + + public void DiscardInBuffer() + { + VerifyConnected(); + int size = client.Available; + byte[] crap = new byte[size]; + Console.WriteLine("UdpSerial DiscardInBuffer {0}",size); + Read(crap, 0, size); + } + + public string ReadLine() { + byte[] temp = new byte[4000]; + int count = 0; + int timeout = 0; + + while (timeout <= 100) + { + if (!this.IsOpen) { break; } + if (this.BytesToRead > 0) + { + byte letter = (byte)this.ReadByte(); + + temp[count] = letter; + + if (letter == '\n') // normal line + { + break; + } + + + count++; + if (count == temp.Length) + break; + timeout = 0; + } else { + timeout++; + System.Threading.Thread.Sleep(5); + } + } + + Array.Resize(ref temp, count + 1); + + return Encoding.ASCII.GetString(temp, 0, temp.Length); + } + + public void Close() + { + if (client.Client.Connected) + { + client.Client.Close(); + client.Close(); + } + + client = new TcpClient(); + } + } +} diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs index 407721b827..b45c37da3a 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs @@ -874,6 +874,11 @@ namespace ArdupilotMega.GCSViews gps.usec = ((ulong)DateTime.Now.Ticks); gps.v = ((float)Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX))); + float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY; + float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX; + + asp.airspeed = ((float)Math.Sqrt((yvec * yvec) + (xvec * xvec))); + } else if (receviedbytes > 0x100) { @@ -1251,7 +1256,7 @@ namespace ArdupilotMega.GCSViews Array.Copy(BitConverter.GetBytes((double)(roll_out * REV_roll)), 0, AeroSimRC, 0, 8); Array.Copy(BitConverter.GetBytes((double)(pitch_out * REV_pitch * -1)), 0, AeroSimRC, 8, 8); Array.Copy(BitConverter.GetBytes((double)(rudder_out * REV_rudder)), 0, AeroSimRC, 16, 8); - Array.Copy(BitConverter.GetBytes((double)(throttle_out)), 0, AeroSimRC, 24, 8); + Array.Copy(BitConverter.GetBytes((double)((throttle_out *2) -1)), 0, AeroSimRC, 24, 8); if (heli) { diff --git a/Tools/ArdupilotMegaPlanner/JoystickSetup.cs b/Tools/ArdupilotMegaPlanner/JoystickSetup.cs index 5f6658f326..d5e8be68cc 100644 --- a/Tools/ArdupilotMegaPlanner/JoystickSetup.cs +++ b/Tools/ArdupilotMegaPlanner/JoystickSetup.cs @@ -413,7 +413,7 @@ namespace ArdupilotMega { try { - if (MainV2.joystick != null) + if (MainV2.joystick != null && MainV2.joystick.enabled == false) MainV2.joystick.UnAcquireJoyStick(); } catch { } diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 6748426041..95f2199f23 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -120,6 +120,7 @@ namespace ArdupilotMega comPort.BaseStream.BaudRate = 115200; CMB_serialport.Items.AddRange(SerialPort.GetPortNames()); + CMB_serialport.Items.Add("TCP"); CMB_serialport.Items.Add("UDP"); if (CMB_serialport.Items.Count > 0) { @@ -225,6 +226,7 @@ namespace ArdupilotMega string oldport = CMB_serialport.Text; CMB_serialport.Items.Clear(); CMB_serialport.Items.AddRange(SerialPort.GetPortNames()); + CMB_serialport.Items.Add("TCP"); CMB_serialport.Items.Add("UDP"); if (CMB_serialport.Items.Contains(oldport)) CMB_serialport.Text = oldport; @@ -564,6 +566,11 @@ namespace ArdupilotMega } else { + if (CMB_serialport.Text == "TCP") + { + comPort.BaseStream = new TcpSerial(); + } + else if (CMB_serialport.Text == "UDP") { comPort.BaseStream = new UdpSerial(); diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index bb90e046be..aa806d298a 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.0.0.0")] -[assembly: AssemblyFileVersion("1.0.81")] +[assembly: AssemblyFileVersion("1.0.82")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/.gitignore b/Tools/ArdupilotMegaPlanner/bin/Release/.gitignore index 5c9527f437..022a200f68 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/.gitignore +++ b/Tools/ArdupilotMegaPlanner/bin/Release/.gitignore @@ -1,4 +1,3 @@ *.pdb -*.xml -*.etag \ No newline at end of file +*.xml \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application index be9caf9d71..bd9f307fa9 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application +++ b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application @@ -11,7 +11,7 @@ - 8RpxcyWYSVdJiQ8oEr1RbFKdPB8= + DxfINYOSh55dWio5h4coAIWRotU=