APM Planner 1.1.45

add tcp and udp port remeber issue 533
add udp listern wait issue 534
fix wp receive on mono part issue 530
allow logging of unknown packets mav 0.9
do_speed_change fix issue 531
remember last video res issue 521
fix issue 528 - ch 6 and ch 7 options

Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj

@@ -243,6 +243,7 @@
     <Compile Include="Controls\ProgressReporterDialogue.designer.cs">
       <DependentUpon>ProgressReporterDialogue.cs</DependentUpon>
     </Compile>
+    <Compile Include="MagCalib.cs" />
     <Compile Include="Radio\3DRradio.cs">
       <SubType>Form</SubType>
     </Compile>

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;

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();

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
             }
 

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))

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<float, float, float> offset = new Tuple<float, float, float>(0, 0, 0);
+            List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>();
+
+
+            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<float,float,float>(
+                                ((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<float, float, float>(
+                                ((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<double> 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<double> ret = new List<double>();
+            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]);
+        }
+
+
+    }
+}

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*");
                 }

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();

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("")]

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);
+    }
+}

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
             {

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

Tools/ArdupilotMegaPlanner/bin/Release/mavcmd.xml

@@ -377,8 +377,8 @@
     </DO_JUMP>
     <DO_CHANGE_SPEED>
       <P1>Type (0=as 1=gs)</P1>
-      <P2>Throttle(%)</P2>
-      <P3>Speed (m/s)</P3>
+      <P2>Speed (m/s)</P2>
+      <P3>Throttle(%)</P3>
       <P4></P4>
       <X></X>
       <Y></Y>

Tools/ArdupilotMegaPlanner/bin/Release/runme

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+gdb --args mono ArdupilotMegaPlanner.exe

Tools/ArdupilotMegaPlanner/mavcmd.xml

@@ -377,8 +377,8 @@
     </DO_JUMP>
     <DO_CHANGE_SPEED>
       <P1>Type (0=as 1=gs)</P1>
-      <P2>Throttle(%)</P2>
-      <P3>Speed (m/s)</P3>
+      <P2>Speed (m/s)</P2>
+      <P3>Throttle(%)</P3>
       <P4></P4>
       <X></X>
       <Y></Y>