diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index 5806ac70d5..8d80840653 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -243,6 +243,7 @@ ProgressReporterDialogue.cs + Form diff --git a/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs b/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs index b9c2fbc5b8..aa42322d42 100644 --- a/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs +++ b/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs @@ -82,6 +82,13 @@ namespace System.IO.Ports string dest = Port; string host = "127.0.0.1"; + + if (ArdupilotMega.MainV2.config["TCP_port"] != null) + dest = ArdupilotMega.MainV2.config["TCP_port"].ToString(); + + if (ArdupilotMega.MainV2.config["TCP_host"] != null) + host = ArdupilotMega.MainV2.config["TCP_host"].ToString(); + if (Windows.Forms.DialogResult.Cancel == ArdupilotMega.Common.InputBox("remote host", "Enter host name/ip (ensure remote end is already started)", ref host)) { throw new Exception("Canceled by request"); @@ -92,6 +99,9 @@ namespace System.IO.Ports } Port = dest; + ArdupilotMega.MainV2.config["TCP_port"] = Port; + ArdupilotMega.MainV2.config["TCP_host"] = host; + client = new TcpClient(host, int.Parse(Port)); client.NoDelay = true; diff --git a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs index e446c4df94..a5ac6dbf51 100644 --- a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs +++ b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs @@ -3,6 +3,7 @@ using System.Text; using System.Net; // dns, ip address using System.Net.Sockets; // tcplistner using log4net; +using ArdupilotMega.Controls; namespace System.IO.Ports { @@ -59,7 +60,7 @@ namespace System.IO.Ports get { return client.Available + rbuffer.Length - rbufferread; } } - public bool IsOpen { get { return client.Client.Connected; } } + public bool IsOpen { get { if (client.Client == null) return false; return client.Client.Connected; } } public bool DtrEnable { @@ -75,22 +76,59 @@ namespace System.IO.Ports return; } + ProgressReporterDialogue frmProgressReporter = new ProgressReporterDialogue + { + StartPosition = System.Windows.Forms.FormStartPosition.CenterScreen, + Text = "Connecting Mavlink UDP" + }; + + frmProgressReporter.DoWork += frmProgressReporter_DoWork; + + frmProgressReporter.UpdateProgressAndStatus(-1, "Connecting Mavlink UDP"); + + ArdupilotMega.MainV2.fixtheme(frmProgressReporter); + + frmProgressReporter.RunBackgroundOperationAsync(); + + + } + + void frmProgressReporter_DoWork(object sender, ArdupilotMega.Controls.ProgressWorkerEventArgs e) + { string dest = Port; + + if (ArdupilotMega.MainV2.config["UDP_port"] != null) + dest = ArdupilotMega.MainV2.config["UDP_port"].ToString(); + ArdupilotMega.Common.InputBox("Listern Port", "Enter Local port (ensure remote end is already sending)", ref dest); Port = dest; + ArdupilotMega.MainV2.config["UDP_port"] = Port; + client = new UdpClient(int.Parse(Port)); - int timeout = 5; - while (timeout > 0) - { - if (BytesToRead > 0) - break; - System.Threading.Thread.Sleep(1000); - timeout--; - } - if (BytesToRead == 0) - return; + while (true) + { + ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Waiting for UDP"); + System.Threading.Thread.Sleep(500); + + if (((ProgressReporterDialogue)sender).doWorkArgs.CancelRequested) + { + ((ProgressReporterDialogue)sender).doWorkArgs.CancelAcknowledged = true; + try + { + client.Close(); + } + catch { } + return; + } + + if (BytesToRead > 0) + break; + } + + if (BytesToRead == 0) + return; try { @@ -98,18 +136,16 @@ namespace System.IO.Ports log.InfoFormat("NetSerial connecting to {0} : {1}", RemoteIpEndPoint.Address, RemoteIpEndPoint.Port); client.Connect(RemoteIpEndPoint); } - catch (Exception e) - { + catch (Exception ex) + { if (client != null && client.Client.Connected) { client.Close(); } - log.Info(e.ToString()); + log.Info(ex.ToString()); System.Windows.Forms.MessageBox.Show("Please check your Firewall settings\nPlease try running this command\n1. Run the following command in an elevated command prompt to disable Windows Firewall temporarily:\n \nNetsh advfirewall set allprofiles state off\n \nNote: This is just for test; please turn it back on with the command 'Netsh advfirewall set allprofiles state on'.\n"); throw new Exception("The socket/serialproxy is closed " + e); } - - return; } void VerifyConnected() @@ -251,7 +287,7 @@ namespace System.IO.Ports public void Close() { - if (client.Client.Connected) + if (client.Client != null && client.Client.Connected) { client.Client.Close(); client.Close(); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index 8a446a2516..04e5e0de87 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -1251,7 +1251,7 @@ namespace ArdupilotMega.GCSViews catch (Exception ex) { error = 1; MessageBox.Show("Error : " + ex.ToString()); } try { - this.BeginInvoke((System.Threading.ThreadStart)delegate() + this.BeginInvoke((MethodInvoker)delegate() { if (error == 0) { @@ -1508,29 +1508,44 @@ namespace ArdupilotMega.GCSViews } } + log.Info("Setting wp params"); + string hold_alt = ((int)((float)param["ALT_HOLD_RTL"] * MainV2.cs.multiplierdist)).ToString(); + log.Info("param ALT_HOLD_RTL " + hold_alt); + if (!hold_alt.Equals("-1")) { TXT_DefaultAlt.Text = hold_alt; } TXT_WPRad.Text = ((int)((float)param["WP_RADIUS"] * MainV2.cs.multiplierdist)).ToString(); + + log.Info("param WP_RADIUS " + TXT_WPRad.Text); + try { - TXT_loiterrad.Text = ((int)((float)param["LOITER_RADIUS"] * MainV2.cs.multiplierdist)).ToString(); + if (param["LOITER_RADIUS"] != null) + TXT_loiterrad.Text = ((int)((float)param["LOITER_RADIUS"] * MainV2.cs.multiplierdist)).ToString(); + + if (param["WP_LOITER_RAD"] != null) + TXT_loiterrad.Text = ((int)((float)param["WP_LOITER_RAD"] * MainV2.cs.multiplierdist)).ToString(); + + log.Info("param LOITER_RADIUS " + TXT_loiterrad.Text); } catch { - TXT_loiterrad.Text = ((int)((float)param["WP_LOITER_RAD"] * MainV2.cs.multiplierdist)).ToString(); + } CHK_holdalt.Checked = Convert.ToBoolean((float)param["ALT_HOLD_RTL"] > 0); + log.Info("param ALT_HOLD_RTL " + CHK_holdalt.Checked.ToString()); } - catch (Exception) { } // if there is no valid home + catch (Exception ex) { log.Info(ex.ToString()); } // if there is no valid home if (Commands.RowCount > 0) { + log.Info("remove home from list"); Commands.Rows.Remove(Commands.Rows[0]); // remove home row } diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index e5cb32bc07..36b70180b2 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -178,6 +178,7 @@ namespace ArdupilotMega lock (objlock) // so we dont have random traffic { + log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate); BaseStream.Open(); @@ -222,7 +223,7 @@ namespace ArdupilotMega // incase we are in setup mode BaseStream.WriteLine("planner\rgcs\r"); - log.Info(DateTime.Now.Millisecond + " start "); + log.Info(DateTime.Now.Millisecond + " Start connect loop "); if (lastbad[0] == '!' && lastbad[1] == 'G' || lastbad[0] == 'G' && lastbad[1] == '!') // waiting for gps lock { @@ -257,7 +258,7 @@ namespace ArdupilotMega try { - log.Info("MAv Data: len " + buffer.Length + " btr " + BaseStream.BytesToRead); + log.Debug("MAv Data: len " + buffer.Length + " btr " + BaseStream.BytesToRead); } catch { } @@ -325,6 +326,7 @@ namespace ArdupilotMega byte[] buffer = readPacket(); if (buffer.Length > 5) { + log.Info("getHB packet received: " + buffer.Length + " btr " + BaseStream.BytesToRead + " type " + buffer[5] ); if (buffer[5] == MAVLINK_MSG_ID_HEARTBEAT) { return buffer; @@ -2074,12 +2076,19 @@ namespace ArdupilotMega if (temp.Length > 5 && temp[1] != MAVLINK_MESSAGE_LENGTHS[temp[5]]) { - log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", temp.Length, temp[5]); + if (MAVLINK_MESSAGE_LENGTHS[temp[5]] == 0) // pass for unknown packets + { + + } + else + { + log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", temp.Length, temp[5]); #if MAVLINK10 if (temp.Length == 11 && temp[0] == 'U' && temp[5] == 0) throw new Exception("Mavlink 0.9 Heartbeat, Please upgrade your AP, This planner is for Mavlink 1.0\n\n"); #endif - return new byte[0]; + return new byte[0]; + } } if (temp.Length < 5 || temp[temp.Length - 1] != (crc >> 8) || temp[temp.Length - 2] != (crc & 0xff)) diff --git a/Tools/ArdupilotMegaPlanner/MagCalib.cs b/Tools/ArdupilotMegaPlanner/MagCalib.cs new file mode 100644 index 0000000000..5edd807b18 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/MagCalib.cs @@ -0,0 +1,138 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Windows.Forms; +using System.IO; + +namespace ArdupilotMega +{ + public class MagCalib + { + + //alglib.lsfit. + + public static void doWork() + { + /* + double[,] x = new double[,] { { -1 }, { -0.8 }, { -0.6 }, { -0.4 }, { -0.2 }, { 0 }, { 0.2 }, { 0.4 }, { 0.6 }, { 0.8 }, { 1.0 } }; + double[] y = new double[] { 0.223130, 0.382893, 0.582748, 0.786628, 0.941765, 1.000000, 0.941765, 0.786628, 0.582748, 0.382893, 0.223130 }; + double[] c = new double[] { 0.3 }; + double epsf = 0; + double epsx = 0.000001; + int maxits = 0; + int info; + alglib.lsfitstate state; + alglib.lsfitreport rep; + double diffstep = 0.0001; + + // + // Fitting without weights + // + alglib.lsfitcreatef(x, y, c, diffstep, out state); + alglib.lsfitsetcond(state, epsf, epsx, maxits); + alglib.lsfitfit(state, function_cx_1_func, null, null); + alglib.lsfitresults(state, out info, out c, out rep); + System.Console.WriteLine("{0}", info); // EXPECTED: 2 + System.Console.WriteLine("{0}", alglib.ap.format(c, 1)); // EXPECTED: [1.5] + */ + + // based of tridge's work + + Tuple offset = new Tuple(0, 0, 0); + List> data = new List>(); + + + 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 + + // gather data + 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(); + + var pack = mine.DebugPacket(packet); + + if (pack.GetType() == typeof(MAVLink.__mavlink_sensor_offsets_t)) + { + offset = new Tuple( + ((MAVLink.__mavlink_sensor_offsets_t)pack).mag_ofs_x, + ((MAVLink.__mavlink_sensor_offsets_t)pack).mag_ofs_y, + ((MAVLink.__mavlink_sensor_offsets_t)pack).mag_ofs_z); + } + else if (pack.GetType() == typeof(MAVLink.__mavlink_raw_imu_t)) + { + data.Add(new Tuple( + ((MAVLink.__mavlink_raw_imu_t)pack).xmag - offset.Item1, + ((MAVLink.__mavlink_raw_imu_t)pack).ymag - offset.Item2, + ((MAVLink.__mavlink_raw_imu_t)pack).zmag - offset.Item3)); + } + + } + + + + + + + //progressBar1.Value = 100; + + mine.logreadmode = false; + mine.logplaybackfile.Close(); + mine.logplaybackfile = null; + + } + } + } + + public static List sphere_error(double[,] p, double[] data) + { + double xofs = p[0, 0]; + double yofs = p[0, 1]; + double zofs = p[0, 2]; + double r = p[0, 3]; + List ret = new List(); + foreach (var d in data) + { + //double x, y, z = d; + //double err = r - Math.Sqrt(Math.Pow((x + xofs), 2) + Math.Pow((y + yofs), 2) + Math.Pow((z + zofs), 2)); + //ret.Add(err); + } + return ret; + } + + public static void function_cx_1_func(double[] c, double[] x, ref double func, object obj) + { + // this callback calculates f(c,x)=exp(-c0*sqr(x0)) + // where x is a position on X-axis and c is adjustable parameter + func = System.Math.Exp(-c[0] * x[0] * x[0]); + } + + + } +} \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 89af4071ef..3ab2c80f11 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -264,7 +264,7 @@ namespace ArdupilotMega if (Directory.Exists("/dev/")) { if (Directory.Exists("/dev/serial/by-id/")) - devs = Directory.GetFiles("/dev/serial/by-id/", "*usb*"); + devs = Directory.GetFiles("/dev/serial/by-id/", "*"); devs = Directory.GetFiles("/dev/", "*ACM*"); devs = Directory.GetFiles("/dev/", "ttyUSB*"); } diff --git a/Tools/ArdupilotMegaPlanner/Program.cs b/Tools/ArdupilotMegaPlanner/Program.cs index 4dd0cef969..a0792ca57c 100644 --- a/Tools/ArdupilotMegaPlanner/Program.cs +++ b/Tools/ArdupilotMegaPlanner/Program.cs @@ -28,6 +28,10 @@ namespace ArdupilotMega Application.Idle += Application_Idle; + //MagCalib.doWork(); + + //return; + //MessageBox.Show("NOTE: This version may break advanced mission scripting"); //Common.linearRegression(); diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index d8f39f2103..a45540e1e7 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.1.44")] +[assembly: AssemblyFileVersion("1.1.45")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Protocol.cs b/Tools/ArdupilotMegaPlanner/Protocol.cs new file mode 100644 index 0000000000..395fb2aeda --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/Protocol.cs @@ -0,0 +1,65 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Collections; +using System.IO; + +namespace ArdupilotMega +{ + partial interface Protocol + { + byte[][] packets { get; set; } + + PointLatLngAlt[] wps { get; set; } + + ICommsSerial BaseStream { get; set; } + + int bps { get; set; } + bool debugmavlink { get; set; } + + DateTime lastvalidpacket {get;set;} + + bool logreadmode { get; set; } + DateTime lastlogread { get; set; } + BinaryReader logplaybackfile { get; set; } + BinaryWriter logfile { get; set; } + + byte sysid { get; set; } + byte compid { get; set; } + Hashtable param { get; set; } + + void UpdateCurrentSettings(ref CurrentState cs); + + void Close(); + + void Open(); + void Open(bool getparams); + + void sendPacket(object indata); + bool Write(string line); + bool setParam(string paramname, float value); + Hashtable getParamList(); + void modifyParamForDisplay(bool fromapm, string paramname, ref float value); + + void stopall(bool forget); + void setWPACK(); + bool setWPCurrent(ushort index); + bool doAction(byte actionid); // MAV_ACTION + + void requestDatastream(byte id, byte hzrate); + byte getWPCount(); + Locationwp getWP(ushort index); + object DebugPacket(byte[] datin); + object DebugPacket(byte[] datin, ref string text); + void setWPTotal(ushort wp_total); + void setWP(Locationwp loc, ushort index, byte frame, byte current); //MAV_FRAME + + void setMountConfigure(byte mountmode, bool stabroll, bool stabpitch, bool stabyaw); //MAV_MOUNT_MODE + void setMountControl(double pa, double pb, double pc, bool islatlng); + void setMode(string modein); + byte[] readPacket(); + + bool translateMode(string modein, ref object navmode, ref object mode); + } +} diff --git a/Tools/ArdupilotMegaPlanner/Speech.cs b/Tools/ArdupilotMegaPlanner/Speech.cs index 7e5c50ba87..baa8cd0b0e 100644 --- a/Tools/ArdupilotMegaPlanner/Speech.cs +++ b/Tools/ArdupilotMegaPlanner/Speech.cs @@ -11,20 +11,21 @@ namespace ArdupilotMega SpeechSynthesizer _speechwindows; System.Diagnostics.Process _speechlinux; + System.Speech.Synthesis.SynthesizerState _state = SynthesizerState.Ready; + bool MONO = false; public SynthesizerState State { get { if (MONO) { - return SynthesizerState.Ready; + return _state; } else { return _speechwindows.State; } - } - private set { } + } } public Speech() @@ -34,8 +35,7 @@ namespace ArdupilotMega if (MONO) { - _speechlinux = new System.Diagnostics.Process(); - _speechlinux.StartInfo.FileName = "festival"; + _state = SynthesizerState.Ready; } else { @@ -47,7 +47,14 @@ namespace ArdupilotMega { if (MONO) { - + if (_speechlinux == null || _speechlinux.HasExited) + { + _state = SynthesizerState.Speaking; + _speechlinux = new System.Diagnostics.Process(); + _speechlinux.StartInfo.FileName = "echo " + text + " | festival --tts"; + _speechlinux.Start(); + _speechlinux.Exited += new EventHandler(_speechlinux_Exited); + } } else { @@ -55,11 +62,17 @@ namespace ArdupilotMega } } + void _speechlinux_Exited(object sender, EventArgs e) + { + _state = SynthesizerState.Ready; + } + public void SpeakAsyncCancelAll() { if (MONO) { - + _speechlinux.Close(); + _state = SynthesizerState.Ready; } else { diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/.gdbinit b/Tools/ArdupilotMegaPlanner/bin/Release/.gdbinit new file mode 100644 index 0000000000..c8b95361b0 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/bin/Release/.gdbinit @@ -0,0 +1,29 @@ +handle SIGXCPU SIG33 SIG35 SIGPWR nostop noprint + +define mono_backtrace + select-frame 0 + set $i = 0 + while ($i < $arg0) + set $foo = (char*) mono_pmip ($pc) + if ($foo) + printf "#%d %p in %s\n", $i, $pc, $foo + else + frame + end + up-silently + set $i = $i + 1 + end +end + + +define mono_stack + set $mono_thread = mono_thread_current () + if ($mono_thread == 0x00) + printf "No mono thread associated with this thread\n" + else + set $ucp = malloc (sizeof (ucontext_t)) + call (void) getcontext ($ucp) + call (void) mono_print_thread_dump ($ucp) + call (void) free ($ucp) + end +end diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb index 39326966bc..1edad2b768 100644 Binary files a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb and b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb differ diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/mavcmd.xml b/Tools/ArdupilotMegaPlanner/bin/Release/mavcmd.xml index a1912a3368..1ae20b7986 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/mavcmd.xml +++ b/Tools/ArdupilotMegaPlanner/bin/Release/mavcmd.xml @@ -377,8 +377,8 @@ Type (0=as 1=gs) - Throttle(%) - Speed (m/s) + Speed (m/s) + Throttle(%) diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/runme b/Tools/ArdupilotMegaPlanner/bin/Release/runme new file mode 100644 index 0000000000..3f0a4f763d --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/bin/Release/runme @@ -0,0 +1,3 @@ +#!/bin/bash + +gdb --args mono ArdupilotMegaPlanner.exe diff --git a/Tools/ArdupilotMegaPlanner/mavcmd.xml b/Tools/ArdupilotMegaPlanner/mavcmd.xml index a1912a3368..1ae20b7986 100644 --- a/Tools/ArdupilotMegaPlanner/mavcmd.xml +++ b/Tools/ArdupilotMegaPlanner/mavcmd.xml @@ -377,8 +377,8 @@ Type (0=as 1=gs) - Throttle(%) - Speed (m/s) + Speed (m/s) + Throttle(%)