APM Planner 1.1.3

add kml overlay to flight data screen
remember last window location
websockets functioning.
add sonar types/optical flow hardware options

Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj

@@ -578,6 +578,7 @@
     <Content Include="mavcmd.xml">
       <CopyToOutputDirectory>Always</CopyToOutputDirectory>
     </Content>
+    <None Include="Resources\BR-0016-01-3T.jpg" />
     <Content Include="Resources\MAVCmd.zh-Hans.txt" />
     <None Include="Resources\MAVParam.txt" />
     <Content Include="Resources\MAVParam.zh-Hans.txt" />

Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs

@@ -349,6 +349,9 @@ namespace ArdupilotMega.GCSViews
                 {
                     try
                     {
+                        if (ctl.GetType() == typeof(NumericUpDown))
+                        {
+
                             NumericUpDown thisctl = ((NumericUpDown)ctl);
                             thisctl.Maximum = 9000;
                             thisctl.Minimum = -9000;
@@ -377,17 +380,19 @@ namespace ArdupilotMega.GCSViews
                                 catch { }
                             }
                             thisctl.Validated += new EventHandler(EEPROM_View_float_TextChanged);
-                    }
-                    catch { }
 
-                    try
+                        }
+                        else if (ctl.GetType() == typeof(ComboBox))
                         {
+
                             ComboBox thisctl = ((ComboBox)ctl);
 
                             thisctl.SelectedIndex = (int)(float)param[value];
 
                             thisctl.Validated += new EventHandler(ComboBox_Validated);
-                    } catch {}
+                        }
+                    }
+                    catch { }
 
                 }
                 if (text.Length == 0)

Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs

@@ -59,6 +59,8 @@ namespace ArdupilotMega.GCSViews
         CurveItem list9curve;
         CurveItem list10curve;
 
+        internal static GMapOverlay kmlpolygons;
+
         bool huddropout = false;
         bool huddropoutresize = false;
 
@@ -154,6 +156,12 @@ namespace ArdupilotMega.GCSViews
 
             gMapControl1.Zoom = 3;
 
+            gMapControl1.RoutesEnabled = true;
+            gMapControl1.PolygonsEnabled = true;
+
+            kmlpolygons = new GMapOverlay(gMapControl1, "kmlpolygons");
+            gMapControl1.Overlays.Add(kmlpolygons);
+
             polygons = new GMapOverlay(gMapControl1, "polygons");
             gMapControl1.Overlays.Add(polygons);
 

Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs

@@ -630,6 +630,16 @@ namespace ArdupilotMega.GCSViews
 
             config(false);
 
+            if (MainV2.HomeLocation.Lat != 0 && MainV2.HomeLocation.Lng != 0)
+            {
+                TXT_homelat.Text = MainV2.HomeLocation.Lat.ToString();
+
+                TXT_homelng.Text = MainV2.HomeLocation.Lng.ToString();
+
+                TXT_homealt.Text = MainV2.HomeLocation.Alt.ToString();
+            }
+
+
             quickadd = false;
 
             if (MainV2.config["WMSserver"] != null)
@@ -700,6 +710,8 @@ namespace ArdupilotMega.GCSViews
             {
                 GMapPolygon kmlpolygon = new GMapPolygon(new List<PointLatLng>(), "kmlpolygon");
 
+                kmlpolygon.Stroke.Color = Color.Purple;
+
                 foreach (var loc in polygon.OuterBoundary.LinearRing.Coordinates)
                 {
                     kmlpolygon.Points.Add(new PointLatLng(loc.Latitude, loc.Longitude));
@@ -711,6 +723,8 @@ namespace ArdupilotMega.GCSViews
             {
                 GMapRoute kmlroute = new GMapRoute(new List<PointLatLng>(), "kmlroute");
 
+                kmlroute.Stroke.Color = Color.Purple;
+
                 foreach (var loc in ls.Coordinates)
                 {
                     kmlroute.Points.Add(new PointLatLng(loc.Latitude, loc.Longitude));
@@ -1514,15 +1528,6 @@ namespace ArdupilotMega.GCSViews
                 {
                     switch (key)
                     {
-                        case "TXT_homelat":
-                            TXT_homelat.Text = ArdupilotMega.MainV2.config[key].ToString();
-                            break;
-                        case "TXT_homelng":
-                            TXT_homelng.Text = ArdupilotMega.MainV2.config[key].ToString();
-                            break;
-                        case "TXT_homealt":
-                            TXT_homealt.Text = ArdupilotMega.MainV2.config[key].ToString();
-                            break;
                         case "TXT_WPRad":
                             TXT_WPRad.Text = ArdupilotMega.MainV2.config[key].ToString();
                             break;
@@ -1652,18 +1657,34 @@ namespace ArdupilotMega.GCSViews
         private void TXT_homelat_TextChanged(object sender, EventArgs e)
         {
             sethome = false;
+            try
+            {
+                MainV2.HomeLocation.Lat = double.Parse(TXT_homelat.Text);
+            }
+            catch { }
             writeKML();
+
         }
 
         private void TXT_homelng_TextChanged(object sender, EventArgs e)
         {
             sethome = false;
+            try
+            {
+                MainV2.HomeLocation.Lng = double.Parse(TXT_homelng.Text);
+            }
+            catch { }
             writeKML();
         }
 
         private void TXT_homealt_TextChanged(object sender, EventArgs e)
         {
             sethome = false;
+            try
+            {
+                MainV2.HomeLocation.Alt = double.Parse(TXT_homealt.Text);
+            }
+            catch { }
             writeKML();
         }
 
@@ -1793,8 +1814,6 @@ namespace ArdupilotMega.GCSViews
         // polygons
         GMapPolygon polygon;
         GMapPolygon drawnpolygon;
-
-        //static GMapRoute route;
         GMapOverlay kmlpolygons;
 
         // layers
@@ -2877,6 +2896,9 @@ namespace ArdupilotMega.GCSViews
                     kmlpolygons.Polygons.Clear();
                     kmlpolygons.Routes.Clear();
 
+                    FlightData.kmlpolygons.Routes.Clear();
+                    FlightData.kmlpolygons.Polygons.Clear();
+
                     string kml = new StreamReader(File.OpenRead(file)).ReadToEnd();
 
                     kml = kml.Replace("<Snippet/>", "");
@@ -2886,6 +2908,18 @@ namespace ArdupilotMega.GCSViews
                     parser.ElementAdded += parser_ElementAdded;
                     parser.ParseString(kml,true);
 
+                    if (DialogResult.Yes == MessageBox.Show("Do you want to load this into the flight data screen?", "Load data", MessageBoxButtons.YesNo))
+                    {
+                        foreach (var temp in kmlpolygons.Polygons)
+                        {
+                            FlightData.kmlpolygons.Polygons.Add(temp);
+                        }
+                        foreach (var temp in kmlpolygons.Routes)
+                        {
+                            FlightData.kmlpolygons.Routes.Add(temp);
+                        }
+                    }
+
                 }
                 catch (Exception ex) { MessageBox.Show("Bad KML File :" + ex.ToString()); }
             }

Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs

@@ -251,7 +251,7 @@ namespace ArdupilotMega.GCSViews
                         {
                             System.Diagnostics.ProcessStartInfo _procstartinfo = new System.Diagnostics.ProcessStartInfo();
                             _procstartinfo.WorkingDirectory = Path.GetDirectoryName(Application.ExecutablePath);
-                            _procstartinfo.Arguments = "--realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=jsbsim/fgout.xml --script=jsbsim/rascal_test.xml";
+                            _procstartinfo.Arguments = "--realtime --suspend --nice --simulation-rate=50 --logdirectivefile=jsbsim/fgout.xml --script=jsbsim/rascal_test.xml";
                             _procstartinfo.FileName = "JSBSim.exe";
                             // Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar +
 

Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs

@@ -20,6 +20,7 @@ namespace ArdupilotMega.HIL
 
         double hover_throttle;
         double terminal_velocity;
+        double terminal_rotation_rate;
 
         Vector3d old_position;
 
@@ -37,6 +38,7 @@ namespace ArdupilotMega.HIL
             motor_speed = new double[] { 0.0, 0.0, 0.0, 0.0 };
             hover_throttle = 0.37;
             terminal_velocity = 30.0;
+            terminal_rotation_rate = 4 * 360.0;
 
             thrust_scale = (mass * gravity) / (4.0 * hover_throttle);
 
@@ -114,6 +116,11 @@ namespace ArdupilotMega.HIL
 
             //Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
 
+            // rotational resistance   
+            roll_accel -= (roll_rate / terminal_rotation_rate) * 5000.0;
+            pitch_accel -= (pitch_rate / terminal_rotation_rate) * 5000.0;
+            yaw_accel -= (yaw_rate / terminal_rotation_rate) * 400.0;
+
             //# update rotational rates
             roll_rate += roll_accel * delta_time.TotalSeconds;
             pitch_rate += pitch_accel * delta_time.TotalSeconds;

Tools/ArdupilotMegaPlanner/JoystickSetup.cs

@@ -467,6 +467,10 @@ namespace ArdupilotMega
 
         private void CHK_elevons_CheckedChanged(object sender, EventArgs e)
         {
+            if (MainV2.joystick == null)
+            {
+                return;
+            }
             MainV2.joystick.elevons = CHK_elevons.Checked;
         }
 

Tools/ArdupilotMegaPlanner/MainV2.cs

@@ -33,6 +33,11 @@ namespace ArdupilotMega
         const int SW_SHOWNORMAL = 1;
         const int SW_HIDE = 0;
 
+        /// <summary>
+        /// Home Location
+        /// </summary>
+        public static PointLatLngAlt HomeLocation = new PointLatLngAlt();
+
         public static MAVLink comPort = new MAVLink();
         public static string comportname = "";
         public static Hashtable config = new Hashtable();
@@ -171,6 +176,13 @@ namespace ArdupilotMega
 
             try
             {
+                if (config["MainLocX"] != null && config["MainLocY"] != null)
+                {
+                    this.StartPosition = FormStartPosition.Manual;
+                    Point startpos = new Point(int.Parse(config["MainLocX"].ToString()), int.Parse(config["MainLocY"].ToString()));
+                    this.Location = startpos;
+                }
+
                 if (config["MainHeight"] != null)
                     this.Height = int.Parse(config["MainHeight"].ToString());
                 if (config["MainWidth"] != null)
@@ -178,7 +190,6 @@ namespace ArdupilotMega
                 if (config["MainMaximised"] != null)
                     this.WindowState = (FormWindowState)Enum.Parse(typeof(FormWindowState), config["MainMaximised"].ToString());
 
-
                 if (config["CMB_rateattitude"] != null)
                     MainV2.cs.rateattitude = byte.Parse(config["CMB_rateattitude"].ToString());
                 if (config["CMB_rateattitude"] != null)
@@ -191,6 +202,20 @@ namespace ArdupilotMega
                 if (config["speechenable"] != null)
                     MainV2.speechenable = bool.Parse(config["speechenable"].ToString());
 
+
+                try
+                {
+                    if (config["TXT_homelat"] != null)
+                        HomeLocation.Lat = double.Parse(config["TXT_homelat"].ToString());
+
+                    if (config["TXT_homelng"] != null)
+                        HomeLocation.Lng = double.Parse(config["TXT_homelng"].ToString());
+
+                    if (config["TXT_homealt"] != null)
+                        HomeLocation.Alt = double.Parse(config["TXT_homealt"].ToString());
+                }
+                catch { }
+
             }
             catch { }
 
@@ -834,7 +859,7 @@ namespace ArdupilotMega
                     {
                         try
                         {
-                            if (key == "")
+                            if (key == "" || key.Contains("/")) // "/dev/blah"
                                 continue;
                             xmlwriter.WriteElementString(key, config[key].ToString());
 
@@ -1212,6 +1237,24 @@ namespace ArdupilotMega
             t11.Start();
         }
 
+        public static String ComputeWebSocketHandshakeSecurityHash09(String secWebSocketKey)
+        {
+            const String MagicKEY = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11";
+            String secWebSocketAccept = String.Empty;
+
+            // 1. Combine the request Sec-WebSocket-Key with magic key.
+            String ret = secWebSocketKey + MagicKEY;
+
+            // 2. Compute the SHA1 hash
+            System.Security.Cryptography.SHA1 sha = new System.Security.Cryptography.SHA1CryptoServiceProvider();
+            byte[] sha1Hash = sha.ComputeHash(Encoding.UTF8.GetBytes(ret));
+
+            // 3. Base64 encode the hash
+            secWebSocketAccept = Convert.ToBase64String(sha1Hash);
+
+            return secWebSocketAccept;
+        }
+
         /// <summary>          
         /// little web server for sending network link kml's          
         /// </summary>          
@@ -1263,29 +1306,54 @@ namespace ArdupilotMega
 
                     if (url.Contains("websocket"))
                     {
-                        using (var writer = new StreamWriter(stream))
+                        using (var writer = new StreamWriter(stream,Encoding.Default))
                         {
-                            writer.WriteLine("HTTP/1.1 101 Web Socket Protocol Handshake");
+                            writer.WriteLine("HTTP/1.1 101 WebSocket Protocol Handshake");
                             writer.WriteLine("Upgrade: WebSocket");
                             writer.WriteLine("Connection: Upgrade");
-                            writer.WriteLine("WebSocket-Origin: http://localhost:56781/");
                             writer.WriteLine("WebSocket-Location: ws://localhost:56781/websocket/server");
+
+                            int start = head.IndexOf("Sec-WebSocket-Key:") + 19;
+                            int end = head.IndexOf('\r', start);
+                            if (end == -1)
+                                end = head.IndexOf('\n', start);
+                            string accept = ComputeWebSocketHandshakeSecurityHash09(head.Substring(start,end-start));
+
+                            writer.WriteLine("Sec-WebSocket-Accept: " + accept);
+
+                            writer.WriteLine("Server: APM Planner");
+
                             writer.WriteLine("");
 
+                            writer.Flush();
+
                             while (client.Connected)
                             {
                                 System.Threading.Thread.Sleep(200);
                                 Console.WriteLine(stream.DataAvailable + " " + client.Available);
-                                stream.WriteByte(0x00);
+
-                                writer.WriteLine("test from planner");
+                                while (client.Available > 0)
-                                stream.WriteByte(0xff);
+                                {
+                                    Console.Write(stream.ReadByte());
+                                }
+
+                                byte[] packet = new byte[256];
+
+                                string sendme = cs.roll + "," + cs.pitch + "," + cs.yaw;
+
+                                packet[0] = 0x81; // fin - binary
+                                packet[1] = (byte)sendme.Length;
+
+                                int i = 2;
+                                foreach (char ch in sendme)
+                                {
+                                    packet[i++] = (byte)ch;
+                                }
+
+                                stream.Write(packet,0,i);
 
                                 //break;
                             }
-
-                            stream.WriteByte(0x00);
-                            //message
-                            stream.WriteByte(0xff);
                         }
                     }
                     else if (url.Contains(".html"))
@@ -1656,9 +1724,13 @@ namespace ArdupilotMega
                 frm.Show();
                 return true;
             }
-            if (keyData == (Keys.Control | Keys.T)) // for ryan beall
+            if (keyData == (Keys.Control | Keys.T)) // for override connect
+            {
+                try
                 {
                     MainV2.comPort.Open(false);
+                }
+                catch (Exception ex) { MessageBox.Show(ex.ToString()); }
                 return true;
             }
             if (keyData == (Keys.Control | Keys.Y)) // for ryan beall
@@ -1700,6 +1772,7 @@ namespace ArdupilotMega
 
             temp.BackColor = Color.FromArgb(0x26, 0x27, 0x28);
         }
+
         public void changelanguage(CultureInfo ci)
         {
             if (ci != null && !Thread.CurrentThread.CurrentUICulture.Equals(ci))
@@ -1770,6 +1843,9 @@ namespace ArdupilotMega
             config["MainWidth"] = this.Width;
             config["MainMaximised"] = this.WindowState.ToString();
 
+            config["MainLocX"] = this.Location.X.ToString();
+            config["MainLocY"] = this.Location.Y.ToString();
+
             try
             {
                 comPort.logreadmode = false;

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.2")]
+[assembly: AssemblyFileVersion("1.1.3")]
 [assembly: NeutralResourcesLanguageAttribute("")]

Tools/ArdupilotMegaPlanner/Properties/Resources.Designer.cs

@@ -109,6 +109,13 @@ namespace ArdupilotMega.Properties {
             }
         }
         
+        public static System.Drawing.Bitmap BR_0016_01_3T {
+            get {
+                object obj = ResourceManager.GetObject("BR_0016_01_3T", resourceCulture);
+                return ((System.Drawing.Bitmap)(obj));
+            }
+        }
+        
         public static System.Drawing.Bitmap compass {
             get {
                 object obj = ResourceManager.GetObject("compass", resourceCulture);

Tools/ArdupilotMegaPlanner/Properties/Resources.resx

@@ -1207,4 +1207,8 @@
   <data name="new_frames_09" type="System.Resources.ResXFileRef, System.Windows.Forms">
     <value>..\Resources\new frames-09.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
   </data>
+  <assembly alias="System.Windows.Forms" name="System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089" />
+  <data name="BR_0016_01_3T" type="System.Resources.ResXFileRef, System.Windows.Forms">
+    <value>..\Resources\BR-0016-01-3T.jpg;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
+  </data>
 </root>

Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs

@@ -40,7 +40,6 @@
             this.CHK_revch1 = new System.Windows.Forms.CheckBox();
             this.BUT_Calibrateradio = new ArdupilotMega.MyButton();
             this.BAR8 = new ArdupilotMega.HorizontalProgressBar2();
-            this.currentStateBindingSource = new System.Windows.Forms.BindingSource(this.components);
             this.BAR7 = new ArdupilotMega.HorizontalProgressBar2();
             this.BAR6 = new ArdupilotMega.HorizontalProgressBar2();
             this.BAR5 = new ArdupilotMega.HorizontalProgressBar2();
@@ -85,9 +84,6 @@
             this.CHK_enableairspeed = new System.Windows.Forms.CheckBox();
             this.CHK_enablesonar = new System.Windows.Forms.CheckBox();
             this.CHK_enablecompass = new System.Windows.Forms.CheckBox();
-            this.pictureBox4 = new System.Windows.Forms.PictureBox();
-            this.pictureBox3 = new System.Windows.Forms.PictureBox();
-            this.pictureBox1 = new System.Windows.Forms.PictureBox();
             this.tabBattery = new System.Windows.Forms.TabPage();
             this.TXT_ampspervolt = new System.Windows.Forms.TextBox();
             this.TXT_divider = new System.Windows.Forms.TextBox();
@@ -104,13 +100,10 @@
             this.label30 = new System.Windows.Forms.Label();
             this.TXT_battcapacity = new System.Windows.Forms.TextBox();
             this.CMB_batmontype = new System.Windows.Forms.ComboBox();
-            this.pictureBox5 = new System.Windows.Forms.PictureBox();
             this.tabArducopter = new System.Windows.Forms.TabPage();
             this.label28 = new System.Windows.Forms.Label();
             this.label16 = new System.Windows.Forms.Label();
             this.label15 = new System.Windows.Forms.Label();
-            this.pictureBoxQuadX = new System.Windows.Forms.PictureBox();
-            this.pictureBoxQuad = new System.Windows.Forms.PictureBox();
             this.BUT_levelac2 = new ArdupilotMega.MyButton();
             this.tabHeli = new System.Windows.Forms.TabPage();
             this.groupBox3 = new System.Windows.Forms.GroupBox();
@@ -162,22 +155,25 @@
             this.BUT_swash_manual = new ArdupilotMega.MyButton();
             this.HS4 = new ArdupilotMega.HorizontalProgressBar2();
             this.HS3 = new ArdupilotMega.VerticalProgressBar2();
-            this.Gservoloc = new AGaugeApp.AGauge();
             this.toolTip1 = new System.Windows.Forms.ToolTip(this.components);
+            this.currentStateBindingSource = new System.Windows.Forms.BindingSource(this.components);
+            this.pictureBox4 = new System.Windows.Forms.PictureBox();
+            this.pictureBox3 = new System.Windows.Forms.PictureBox();
+            this.pictureBox1 = new System.Windows.Forms.PictureBox();
+            this.pictureBox5 = new System.Windows.Forms.PictureBox();
+            this.pictureBoxQuadX = new System.Windows.Forms.PictureBox();
+            this.pictureBoxQuad = new System.Windows.Forms.PictureBox();
+            this.Gservoloc = new AGaugeApp.AGauge();
+            this.CHK_enableoptflow = new System.Windows.Forms.CheckBox();
+            this.pictureBox2 = new System.Windows.Forms.PictureBox();
+            this.CMB_sonartype = new System.Windows.Forms.ComboBox();
             this.tabControl1.SuspendLayout();
             this.tabReset.SuspendLayout();
             this.tabRadioIn.SuspendLayout();
-            ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit();
             this.tabModes.SuspendLayout();
             this.tabHardware.SuspendLayout();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBox4)).BeginInit();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBox3)).BeginInit();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBox1)).BeginInit();
             this.tabBattery.SuspendLayout();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBox5)).BeginInit();
             this.tabArducopter.SuspendLayout();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuadX)).BeginInit();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).BeginInit();
             this.tabHeli.SuspendLayout();
             this.groupBox3.SuspendLayout();
             this.groupBox2.SuspendLayout();
@@ -186,6 +182,14 @@
             ((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.HS2_TRIM)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.HS1_TRIM)).BeginInit();
+            ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBox4)).BeginInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBox3)).BeginInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBox1)).BeginInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBox5)).BeginInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuadX)).BeginInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).BeginInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBox2)).BeginInit();
             this.SuspendLayout();
             // 
             // tabControl1
@@ -286,10 +290,6 @@
             this.BAR8.Value = 1500;
             this.BAR8.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
             // 
-            // currentStateBindingSource
-            // 
-            this.currentStateBindingSource.DataSource = typeof(ArdupilotMega.CurrentState);
-            // 
             // BAR7
             // 
             this.BAR7.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
@@ -611,6 +611,9 @@
             // tabHardware
             // 
             this.tabHardware.BackColor = System.Drawing.Color.DarkRed;
+            this.tabHardware.Controls.Add(this.CMB_sonartype);
+            this.tabHardware.Controls.Add(this.CHK_enableoptflow);
+            this.tabHardware.Controls.Add(this.pictureBox2);
             this.tabHardware.Controls.Add(this.linkLabelmagdec);
             this.tabHardware.Controls.Add(this.label100);
             this.tabHardware.Controls.Add(this.TXT_declination);
@@ -663,32 +666,6 @@
             this.CHK_enablecompass.UseVisualStyleBackColor = true;
             this.CHK_enablecompass.CheckedChanged += new System.EventHandler(this.CHK_enablecompass_CheckedChanged);
             // 
-            // pictureBox4
-            // 
-            this.pictureBox4.BackColor = System.Drawing.Color.White;
-            this.pictureBox4.BackgroundImage = global::ArdupilotMega.Properties.Resources.airspeed;
-            resources.ApplyResources(this.pictureBox4, "pictureBox4");
-            this.pictureBox4.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
-            this.pictureBox4.Name = "pictureBox4";
-            this.pictureBox4.TabStop = false;
-            // 
-            // pictureBox3
-            // 
-            this.pictureBox3.BackColor = System.Drawing.Color.White;
-            this.pictureBox3.BackgroundImage = global::ArdupilotMega.Properties.Resources.sonar;
-            resources.ApplyResources(this.pictureBox3, "pictureBox3");
-            this.pictureBox3.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
-            this.pictureBox3.Name = "pictureBox3";
-            this.pictureBox3.TabStop = false;
-            // 
-            // pictureBox1
-            // 
-            this.pictureBox1.BackgroundImage = global::ArdupilotMega.Properties.Resources.compass;
-            resources.ApplyResources(this.pictureBox1, "pictureBox1");
-            this.pictureBox1.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
-            this.pictureBox1.Name = "pictureBox1";
-            this.pictureBox1.TabStop = false;
-            // 
             // tabBattery
             // 
             this.tabBattery.Controls.Add(this.TXT_ampspervolt);
@@ -807,23 +784,14 @@
             this.CMB_batmontype.Name = "CMB_batmontype";
             this.CMB_batmontype.SelectedIndexChanged += new System.EventHandler(this.CMB_batmontype_SelectedIndexChanged);
             // 
-            // pictureBox5
-            // 
-            this.pictureBox5.BackColor = System.Drawing.Color.White;
-            this.pictureBox5.BackgroundImage = global::ArdupilotMega.Properties.Resources.attocurrent;
-            resources.ApplyResources(this.pictureBox5, "pictureBox5");
-            this.pictureBox5.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
-            this.pictureBox5.Name = "pictureBox5";
-            this.pictureBox5.TabStop = false;
-            // 
             // tabArducopter
             // 
             this.tabArducopter.Controls.Add(this.label28);
             this.tabArducopter.Controls.Add(this.label16);
             this.tabArducopter.Controls.Add(this.label15);
+            this.tabArducopter.Controls.Add(this.BUT_levelac2);
             this.tabArducopter.Controls.Add(this.pictureBoxQuadX);
             this.tabArducopter.Controls.Add(this.pictureBoxQuad);
-            this.tabArducopter.Controls.Add(this.BUT_levelac2);
             resources.ApplyResources(this.tabArducopter, "tabArducopter");
             this.tabArducopter.Name = "tabArducopter";
             this.tabArducopter.UseVisualStyleBackColor = true;
@@ -843,24 +811,6 @@
             resources.ApplyResources(this.label15, "label15");
             this.label15.Name = "label15";
             // 
-            // pictureBoxQuadX
-            // 
-            this.pictureBoxQuadX.Cursor = System.Windows.Forms.Cursors.Hand;
-            this.pictureBoxQuadX.Image = global::ArdupilotMega.Properties.Resources.frames_04;
-            resources.ApplyResources(this.pictureBoxQuadX, "pictureBoxQuadX");
-            this.pictureBoxQuadX.Name = "pictureBoxQuadX";
-            this.pictureBoxQuadX.TabStop = false;
-            this.pictureBoxQuadX.Click += new System.EventHandler(this.pictureBoxQuadX_Click);
-            // 
-            // pictureBoxQuad
-            // 
-            this.pictureBoxQuad.Cursor = System.Windows.Forms.Cursors.Hand;
-            this.pictureBoxQuad.Image = global::ArdupilotMega.Properties.Resources.frames_03;
-            resources.ApplyResources(this.pictureBoxQuad, "pictureBoxQuad");
-            this.pictureBoxQuad.Name = "pictureBoxQuad";
-            this.pictureBoxQuad.TabStop = false;
-            this.pictureBoxQuad.Click += new System.EventHandler(this.pictureBoxQuad_Click);
-            // 
             // BUT_levelac2
             // 
             resources.ApplyResources(this.BUT_levelac2, "BUT_levelac2");
@@ -1294,6 +1244,63 @@
             this.HS3.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(148)))), ((int)(((byte)(193)))), ((int)(((byte)(31)))));
             this.HS3.Paint += new System.Windows.Forms.PaintEventHandler(this.HS3_Paint);
             // 
+            // currentStateBindingSource
+            // 
+            this.currentStateBindingSource.DataSource = typeof(ArdupilotMega.CurrentState);
+            // 
+            // pictureBox4
+            // 
+            this.pictureBox4.BackColor = System.Drawing.Color.White;
+            this.pictureBox4.BackgroundImage = global::ArdupilotMega.Properties.Resources.airspeed;
+            resources.ApplyResources(this.pictureBox4, "pictureBox4");
+            this.pictureBox4.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
+            this.pictureBox4.Name = "pictureBox4";
+            this.pictureBox4.TabStop = false;
+            // 
+            // pictureBox3
+            // 
+            this.pictureBox3.BackColor = System.Drawing.Color.White;
+            this.pictureBox3.BackgroundImage = global::ArdupilotMega.Properties.Resources.sonar;
+            resources.ApplyResources(this.pictureBox3, "pictureBox3");
+            this.pictureBox3.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
+            this.pictureBox3.Name = "pictureBox3";
+            this.pictureBox3.TabStop = false;
+            // 
+            // pictureBox1
+            // 
+            this.pictureBox1.BackgroundImage = global::ArdupilotMega.Properties.Resources.compass;
+            resources.ApplyResources(this.pictureBox1, "pictureBox1");
+            this.pictureBox1.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
+            this.pictureBox1.Name = "pictureBox1";
+            this.pictureBox1.TabStop = false;
+            // 
+            // pictureBox5
+            // 
+            this.pictureBox5.BackColor = System.Drawing.Color.White;
+            this.pictureBox5.BackgroundImage = global::ArdupilotMega.Properties.Resources.attocurrent;
+            resources.ApplyResources(this.pictureBox5, "pictureBox5");
+            this.pictureBox5.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
+            this.pictureBox5.Name = "pictureBox5";
+            this.pictureBox5.TabStop = false;
+            // 
+            // pictureBoxQuadX
+            // 
+            this.pictureBoxQuadX.Cursor = System.Windows.Forms.Cursors.Hand;
+            this.pictureBoxQuadX.Image = global::ArdupilotMega.Properties.Resources.frames_04;
+            resources.ApplyResources(this.pictureBoxQuadX, "pictureBoxQuadX");
+            this.pictureBoxQuadX.Name = "pictureBoxQuadX";
+            this.pictureBoxQuadX.TabStop = false;
+            this.pictureBoxQuadX.Click += new System.EventHandler(this.pictureBoxQuadX_Click);
+            // 
+            // pictureBoxQuad
+            // 
+            this.pictureBoxQuad.Cursor = System.Windows.Forms.Cursors.Hand;
+            this.pictureBoxQuad.Image = global::ArdupilotMega.Properties.Resources.frames_03;
+            resources.ApplyResources(this.pictureBoxQuad, "pictureBoxQuad");
+            this.pictureBoxQuad.Name = "pictureBoxQuad";
+            this.pictureBoxQuad.TabStop = false;
+            this.pictureBoxQuad.Click += new System.EventHandler(this.pictureBoxQuad_Click);
+            // 
             // Gservoloc
             // 
             this.Gservoloc.BackColor = System.Drawing.Color.Transparent;
@@ -1437,6 +1444,33 @@
             this.Gservoloc.Value2 = 180F;
             this.Gservoloc.Value3 = 0F;
             // 
+            // CHK_enableoptflow
+            // 
+            resources.ApplyResources(this.CHK_enableoptflow, "CHK_enableoptflow");
+            this.CHK_enableoptflow.Name = "CHK_enableoptflow";
+            this.CHK_enableoptflow.UseVisualStyleBackColor = true;
+            this.CHK_enableoptflow.CheckedChanged += new System.EventHandler(this.CHK_enableoptflow_CheckedChanged);
+            // 
+            // pictureBox2
+            // 
+            this.pictureBox2.BackColor = System.Drawing.Color.White;
+            this.pictureBox2.BackgroundImage = global::ArdupilotMega.Properties.Resources.BR_0016_01_3T;
+            resources.ApplyResources(this.pictureBox2, "pictureBox2");
+            this.pictureBox2.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
+            this.pictureBox2.Name = "pictureBox2";
+            this.pictureBox2.TabStop = false;
+            // 
+            // CMB_sonartype
+            // 
+            this.CMB_sonartype.FormattingEnabled = true;
+            this.CMB_sonartype.Items.AddRange(new object[] {
+            resources.GetString("CMB_sonartype.Items"),
+            resources.GetString("CMB_sonartype.Items1"),
+            resources.GetString("CMB_sonartype.Items2")});
+            resources.ApplyResources(this.CMB_sonartype, "CMB_sonartype");
+            this.CMB_sonartype.Name = "CMB_sonartype";
+            this.CMB_sonartype.SelectedIndexChanged += new System.EventHandler(this.CMB_sonartype_SelectedIndexChanged);
+            // 
             // Setup
             // 
             resources.ApplyResources(this, "$this");
@@ -1450,21 +1484,14 @@
             this.tabReset.ResumeLayout(false);
             this.tabRadioIn.ResumeLayout(false);
             this.tabRadioIn.PerformLayout();
-            ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).EndInit();
             this.tabModes.ResumeLayout(false);
             this.tabModes.PerformLayout();
             this.tabHardware.ResumeLayout(false);
             this.tabHardware.PerformLayout();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBox4)).EndInit();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBox3)).EndInit();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBox1)).EndInit();
             this.tabBattery.ResumeLayout(false);
             this.tabBattery.PerformLayout();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBox5)).EndInit();
             this.tabArducopter.ResumeLayout(false);
             this.tabArducopter.PerformLayout();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuadX)).EndInit();
-            ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).EndInit();
             this.tabHeli.ResumeLayout(false);
             this.tabHeli.PerformLayout();
             this.groupBox3.ResumeLayout(false);
@@ -1477,6 +1504,14 @@
             ((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.HS2_TRIM)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.HS1_TRIM)).EndInit();
+            ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).EndInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBox4)).EndInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBox3)).EndInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBox1)).EndInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBox5)).EndInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuadX)).EndInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).EndInit();
+            ((System.ComponentModel.ISupportInitialize)(this.pictureBox2)).EndInit();
             this.ResumeLayout(false);
 
         }
@@ -1617,6 +1652,9 @@
         private System.Windows.Forms.GroupBox groupBox3;
         private System.Windows.Forms.Label label46;
         private System.Windows.Forms.Label label45;
+        private System.Windows.Forms.CheckBox CHK_enableoptflow;
+        private System.Windows.Forms.PictureBox pictureBox2;
+        private System.Windows.Forms.ComboBox CMB_sonartype;
 
     }
 }

Tools/ArdupilotMegaPlanner/Setup/Setup.cs

@@ -374,6 +374,11 @@ namespace ArdupilotMega.Setup
 
                 if (MainV2.comPort.param["COMPASS_DEC"] != null)
                     TXT_declination.Text = (float.Parse(MainV2.comPort.param["COMPASS_DEC"].ToString()) * rad2deg).ToString();
+
+                if (MainV2.comPort.param["SONAR_TYPE"] != null)
+                    CMB_sonartype.SelectedIndex = int.Parse(MainV2.comPort.param["SONAR_TYPE"].ToString());
+                
+
                 startup = false;
             }
 
@@ -1192,7 +1197,8 @@ namespace ArdupilotMega.Setup
                     HS4_MIN.Text = HS4.minline.ToString();
                 if (int.Parse(HS4_MAX.Text) < HS4.maxline)
                     HS4_MAX.Text = HS4.maxline.ToString();
-            } catch {}
+            }
+            catch { }
         }
 
         private void HS3_Paint(object sender, PaintEventArgs e)
@@ -1262,5 +1268,42 @@ namespace ArdupilotMega.Setup
             timer.Stop();
             timer.Dispose();
         }
+
+        private void CHK_enableoptflow_CheckedChanged(object sender, EventArgs e)
+        {
+
+            if (startup)
+                return;
+            try
+            {
+                if (MainV2.comPort.param["FLOW_ENABLE"] == null)
+                {
+                    MessageBox.Show("Not Available on " + MainV2.cs.firmware.ToString());
+                }
+                else
+                {
+                    MainV2.comPort.setParam("FLOW_ENABLE", ((CheckBox)sender).Checked == true ? 1 : 0);
+                }
+            }
+            catch { MessageBox.Show("Set FLOW_ENABLE Failed"); }
+        }
+
+        private void CMB_sonartype_SelectedIndexChanged(object sender, EventArgs e)
+        {
+            if (startup)
+                return;
+            try
+            {
+                if (MainV2.comPort.param["SONAR_TYPE"] == null)
+                {
+                    MessageBox.Show("Not Available on " + MainV2.cs.firmware.ToString());
+                }
+                else
+                {
+                    MainV2.comPort.setParam("SONAR_TYPE", ((ComboBox)sender).SelectedIndex);
+                }
+            }
+            catch { MessageBox.Show("Set SONAR_TYPE Failed"); }
+        }
     }
 }

Tools/ArdupilotMegaPlanner/Setup/Setup.resx

@@ -711,6 +711,90 @@
   <data name="&gt;&gt;tabModes.ZOrder" xml:space="preserve">
     <value>2</value>
   </data>
+  <data name="CMB_sonartype.Items" xml:space="preserve">
+    <value>XL-EZ0</value>
+  </data>
+  <data name="CMB_sonartype.Items1" xml:space="preserve">
+    <value>LV-EZ0</value>
+  </data>
+  <data name="CMB_sonartype.Items2" xml:space="preserve">
+    <value>XL-EXL0</value>
+  </data>
+  <data name="CMB_sonartype.Location" type="System.Drawing.Point, System.Drawing">
+    <value>308, 134</value>
+  </data>
+  <data name="CMB_sonartype.Size" type="System.Drawing.Size, System.Drawing">
+    <value>121, 21</value>
+  </data>
+  <data name="CMB_sonartype.TabIndex" type="System.Int32, mscorlib">
+    <value>31</value>
+  </data>
+  <data name="&gt;&gt;CMB_sonartype.Name" xml:space="preserve">
+    <value>CMB_sonartype</value>
+  </data>
+  <data name="&gt;&gt;CMB_sonartype.Type" xml:space="preserve">
+    <value>System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </data>
+  <data name="&gt;&gt;CMB_sonartype.Parent" xml:space="preserve">
+    <value>tabHardware</value>
+  </data>
+  <data name="&gt;&gt;CMB_sonartype.ZOrder" xml:space="preserve">
+    <value>0</value>
+  </data>
+  <data name="CHK_enableoptflow.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
+    <value>NoControl</value>
+  </data>
+  <data name="CHK_enableoptflow.Location" type="System.Drawing.Point, System.Drawing">
+    <value>162, 297</value>
+  </data>
+  <data name="CHK_enableoptflow.Size" type="System.Drawing.Size, System.Drawing">
+    <value>134, 19</value>
+  </data>
+  <data name="CHK_enableoptflow.TabIndex" type="System.Int32, mscorlib">
+    <value>30</value>
+  </data>
+  <data name="CHK_enableoptflow.Text" xml:space="preserve">
+    <value>Enable Optical Flow</value>
+  </data>
+  <data name="&gt;&gt;CHK_enableoptflow.Name" xml:space="preserve">
+    <value>CHK_enableoptflow</value>
+  </data>
+  <data name="&gt;&gt;CHK_enableoptflow.Type" xml:space="preserve">
+    <value>System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </data>
+  <data name="&gt;&gt;CHK_enableoptflow.Parent" xml:space="preserve">
+    <value>tabHardware</value>
+  </data>
+  <data name="&gt;&gt;CHK_enableoptflow.ZOrder" xml:space="preserve">
+    <value>1</value>
+  </data>
+  <data name="pictureBox2.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
+    <value>Zoom</value>
+  </data>
+  <data name="pictureBox2.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
+    <value>NoControl</value>
+  </data>
+  <data name="pictureBox2.Location" type="System.Drawing.Point, System.Drawing">
+    <value>78, 271</value>
+  </data>
+  <data name="pictureBox2.Size" type="System.Drawing.Size, System.Drawing">
+    <value>75, 75</value>
+  </data>
+  <data name="pictureBox2.TabIndex" type="System.Int32, mscorlib">
+    <value>29</value>
+  </data>
+  <data name="&gt;&gt;pictureBox2.Name" xml:space="preserve">
+    <value>pictureBox2</value>
+  </data>
+  <data name="&gt;&gt;pictureBox2.Type" xml:space="preserve">
+    <value>System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </data>
+  <data name="&gt;&gt;pictureBox2.Parent" xml:space="preserve">
+    <value>tabHardware</value>
+  </data>
+  <data name="&gt;&gt;pictureBox2.ZOrder" xml:space="preserve">
+    <value>2</value>
+  </data>
   <data name="linkLabelmagdec.AutoSize" type="System.Boolean, mscorlib">
     <value>True</value>
   </data>
@@ -739,7 +823,7 @@
     <value>tabHardware</value>
   </data>
   <data name="&gt;&gt;linkLabelmagdec.ZOrder" xml:space="preserve">
-    <value>0</value>
+    <value>3</value>
   </data>
   <data name="label100.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
     <value>NoControl</value>
@@ -766,7 +850,7 @@
     <value>tabHardware</value>
   </data>
   <data name="&gt;&gt;label100.ZOrder" xml:space="preserve">
-    <value>1</value>
+    <value>4</value>
   </data>
   <metadata name="toolTip1.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
     <value>214, 17</value>
@@ -793,7 +877,7 @@
     <value>tabHardware</value>
   </data>
   <data name="&gt;&gt;TXT_declination.ZOrder" xml:space="preserve">
-    <value>2</value>
+    <value>5</value>
   </data>
   <data name="CHK_enableairspeed.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
     <value>NoControl</value>
@@ -820,7 +904,7 @@
     <value>tabHardware</value>
   </data>
   <data name="&gt;&gt;CHK_enableairspeed.ZOrder" xml:space="preserve">
-    <value>3</value>
+    <value>6</value>
   </data>
   <data name="CHK_enablesonar.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
     <value>NoControl</value>
@@ -847,7 +931,7 @@
     <value>tabHardware</value>
   </data>
   <data name="&gt;&gt;CHK_enablesonar.ZOrder" xml:space="preserve">
-    <value>4</value>
+    <value>7</value>
   </data>
   <data name="CHK_enablecompass.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
     <value>NoControl</value>
@@ -874,7 +958,7 @@
     <value>tabHardware</value>
   </data>
   <data name="&gt;&gt;CHK_enablecompass.ZOrder" xml:space="preserve">
-    <value>5</value>
+    <value>8</value>
   </data>
   <data name="pictureBox4.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
     <value>Zoom</value>
@@ -901,7 +985,7 @@
     <value>tabHardware</value>
   </data>
   <data name="&gt;&gt;pictureBox4.ZOrder" xml:space="preserve">
-    <value>6</value>
+    <value>9</value>
   </data>
   <data name="pictureBox3.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
     <value>Zoom</value>
@@ -928,7 +1012,7 @@
     <value>tabHardware</value>
   </data>
   <data name="&gt;&gt;pictureBox3.ZOrder" xml:space="preserve">
-    <value>7</value>
+    <value>10</value>
   </data>
   <data name="pictureBox1.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
     <value>Zoom</value>
@@ -961,7 +1045,7 @@
     <value>tabHardware</value>
   </data>
   <data name="&gt;&gt;pictureBox1.ZOrder" xml:space="preserve">
-    <value>8</value>
+    <value>11</value>
   </data>
   <data name="tabHardware.Location" type="System.Drawing.Point, System.Drawing">
     <value>4, 22</value>
@@ -1245,6 +1329,18 @@
   <data name="&gt;&gt;label15.ZOrder" xml:space="preserve">
     <value>2</value>
   </data>
+  <data name="&gt;&gt;BUT_levelac2.Name" xml:space="preserve">
+    <value>BUT_levelac2</value>
+  </data>
+  <data name="&gt;&gt;BUT_levelac2.Type" xml:space="preserve">
+    <value>ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc</value>
+  </data>
+  <data name="&gt;&gt;BUT_levelac2.Parent" xml:space="preserve">
+    <value>tabArducopter</value>
+  </data>
+  <data name="&gt;&gt;BUT_levelac2.ZOrder" xml:space="preserve">
+    <value>3</value>
+  </data>
   <data name="&gt;&gt;pictureBoxQuadX.Name" xml:space="preserve">
     <value>pictureBoxQuadX</value>
   </data>
@@ -1255,7 +1351,7 @@
     <value>tabArducopter</value>
   </data>
   <data name="&gt;&gt;pictureBoxQuadX.ZOrder" xml:space="preserve">
-    <value>3</value>
+    <value>4</value>
   </data>
   <data name="&gt;&gt;pictureBoxQuad.Name" xml:space="preserve">
     <value>pictureBoxQuad</value>
@@ -1267,18 +1363,6 @@
     <value>tabArducopter</value>
   </data>
   <data name="&gt;&gt;pictureBoxQuad.ZOrder" xml:space="preserve">
-    <value>4</value>
-  </data>
-  <data name="&gt;&gt;BUT_levelac2.Name" xml:space="preserve">
-    <value>BUT_levelac2</value>
-  </data>
-  <data name="&gt;&gt;BUT_levelac2.Type" xml:space="preserve">
-    <value>ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc</value>
-  </data>
-  <data name="&gt;&gt;BUT_levelac2.Parent" xml:space="preserve">
-    <value>tabArducopter</value>
-  </data>
-  <data name="&gt;&gt;BUT_levelac2.ZOrder" xml:space="preserve">
     <value>5</value>
   </data>
   <data name="tabArducopter.Location" type="System.Drawing.Point, System.Drawing">
@@ -1983,9 +2067,6 @@
   <data name="&gt;&gt;BAR8.ZOrder" xml:space="preserve">
     <value>5</value>
   </data>
-  <metadata name="currentStateBindingSource.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
-    <value>17, 17</value>
-  </metadata>
   <data name="BAR7.Location" type="System.Drawing.Point, System.Drawing">
     <value>446, 185</value>
   </data>
@@ -3414,33 +3495,6 @@
   <data name="&gt;&gt;CMB_batmontype.ZOrder" xml:space="preserve">
     <value>14</value>
   </data>
-  <data name="pictureBox5.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
-    <value>Zoom</value>
-  </data>
-  <data name="pictureBox5.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
-    <value>NoControl</value>
-  </data>
-  <data name="pictureBox5.Location" type="System.Drawing.Point, System.Drawing">
-    <value>31, 21</value>
-  </data>
-  <data name="pictureBox5.Size" type="System.Drawing.Size, System.Drawing">
-    <value>75, 75</value>
-  </data>
-  <data name="pictureBox5.TabIndex" type="System.Int32, mscorlib">
-    <value>2</value>
-  </data>
-  <data name="&gt;&gt;pictureBox5.Name" xml:space="preserve">
-    <value>pictureBox5</value>
-  </data>
-  <data name="&gt;&gt;pictureBox5.Type" xml:space="preserve">
-    <value>System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
-  </data>
-  <data name="&gt;&gt;pictureBox5.Parent" xml:space="preserve">
-    <value>tabBattery</value>
-  </data>
-  <data name="&gt;&gt;pictureBox5.ZOrder" xml:space="preserve">
-    <value>15</value>
-  </data>
   <data name="label28.AutoSize" type="System.Boolean, mscorlib">
     <value>True</value>
   </data>
@@ -3532,60 +3586,6 @@ will work with hexa's etc</value>
   <data name="&gt;&gt;label15.ZOrder" xml:space="preserve">
     <value>2</value>
   </data>
-  <data name="pictureBoxQuadX.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
-    <value>NoControl</value>
-  </data>
-  <data name="pictureBoxQuadX.Location" type="System.Drawing.Point, System.Drawing">
-    <value>319, 140</value>
-  </data>
-  <data name="pictureBoxQuadX.Size" type="System.Drawing.Size, System.Drawing">
-    <value>190, 190</value>
-  </data>
-  <data name="pictureBoxQuadX.SizeMode" type="System.Windows.Forms.PictureBoxSizeMode, System.Windows.Forms">
-    <value>Zoom</value>
-  </data>
-  <data name="pictureBoxQuadX.TabIndex" type="System.Int32, mscorlib">
-    <value>5</value>
-  </data>
-  <data name="&gt;&gt;pictureBoxQuadX.Name" xml:space="preserve">
-    <value>pictureBoxQuadX</value>
-  </data>
-  <data name="&gt;&gt;pictureBoxQuadX.Type" xml:space="preserve">
-    <value>System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
-  </data>
-  <data name="&gt;&gt;pictureBoxQuadX.Parent" xml:space="preserve">
-    <value>tabArducopter</value>
-  </data>
-  <data name="&gt;&gt;pictureBoxQuadX.ZOrder" xml:space="preserve">
-    <value>3</value>
-  </data>
-  <data name="pictureBoxQuad.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
-    <value>NoControl</value>
-  </data>
-  <data name="pictureBoxQuad.Location" type="System.Drawing.Point, System.Drawing">
-    <value>112, 140</value>
-  </data>
-  <data name="pictureBoxQuad.Size" type="System.Drawing.Size, System.Drawing">
-    <value>190, 190</value>
-  </data>
-  <data name="pictureBoxQuad.SizeMode" type="System.Windows.Forms.PictureBoxSizeMode, System.Windows.Forms">
-    <value>Zoom</value>
-  </data>
-  <data name="pictureBoxQuad.TabIndex" type="System.Int32, mscorlib">
-    <value>4</value>
-  </data>
-  <data name="&gt;&gt;pictureBoxQuad.Name" xml:space="preserve">
-    <value>pictureBoxQuad</value>
-  </data>
-  <data name="&gt;&gt;pictureBoxQuad.Type" xml:space="preserve">
-    <value>System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
-  </data>
-  <data name="&gt;&gt;pictureBoxQuad.Parent" xml:space="preserve">
-    <value>tabArducopter</value>
-  </data>
-  <data name="&gt;&gt;pictureBoxQuad.ZOrder" xml:space="preserve">
-    <value>4</value>
-  </data>
   <data name="BUT_levelac2.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
     <value>NoControl</value>
   </data>
@@ -3611,7 +3611,7 @@ will work with hexa's etc</value>
     <value>tabArducopter</value>
   </data>
   <data name="&gt;&gt;BUT_levelac2.ZOrder" xml:space="preserve">
-    <value>5</value>
+    <value>3</value>
   </data>
   <data name="&gt;&gt;label46.Name" xml:space="preserve">
     <value>label46</value>
@@ -5083,6 +5083,93 @@ will work with hexa's etc</value>
   <data name="&gt;&gt;HS3.ZOrder" xml:space="preserve">
     <value>34</value>
   </data>
+  <metadata name="toolTip1.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
+    <value>214, 17</value>
+  </metadata>
+  <metadata name="currentStateBindingSource.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
+    <value>17, 17</value>
+  </metadata>
+  <data name="pictureBox5.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
+    <value>Zoom</value>
+  </data>
+  <data name="pictureBox5.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
+    <value>NoControl</value>
+  </data>
+  <data name="pictureBox5.Location" type="System.Drawing.Point, System.Drawing">
+    <value>31, 21</value>
+  </data>
+  <data name="pictureBox5.Size" type="System.Drawing.Size, System.Drawing">
+    <value>75, 75</value>
+  </data>
+  <data name="pictureBox5.TabIndex" type="System.Int32, mscorlib">
+    <value>2</value>
+  </data>
+  <data name="&gt;&gt;pictureBox5.Name" xml:space="preserve">
+    <value>pictureBox5</value>
+  </data>
+  <data name="&gt;&gt;pictureBox5.Type" xml:space="preserve">
+    <value>System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </data>
+  <data name="&gt;&gt;pictureBox5.Parent" xml:space="preserve">
+    <value>tabBattery</value>
+  </data>
+  <data name="&gt;&gt;pictureBox5.ZOrder" xml:space="preserve">
+    <value>15</value>
+  </data>
+  <data name="pictureBoxQuadX.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
+    <value>NoControl</value>
+  </data>
+  <data name="pictureBoxQuadX.Location" type="System.Drawing.Point, System.Drawing">
+    <value>319, 140</value>
+  </data>
+  <data name="pictureBoxQuadX.Size" type="System.Drawing.Size, System.Drawing">
+    <value>190, 190</value>
+  </data>
+  <data name="pictureBoxQuadX.SizeMode" type="System.Windows.Forms.PictureBoxSizeMode, System.Windows.Forms">
+    <value>Zoom</value>
+  </data>
+  <data name="pictureBoxQuadX.TabIndex" type="System.Int32, mscorlib">
+    <value>5</value>
+  </data>
+  <data name="&gt;&gt;pictureBoxQuadX.Name" xml:space="preserve">
+    <value>pictureBoxQuadX</value>
+  </data>
+  <data name="&gt;&gt;pictureBoxQuadX.Type" xml:space="preserve">
+    <value>System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </data>
+  <data name="&gt;&gt;pictureBoxQuadX.Parent" xml:space="preserve">
+    <value>tabArducopter</value>
+  </data>
+  <data name="&gt;&gt;pictureBoxQuadX.ZOrder" xml:space="preserve">
+    <value>4</value>
+  </data>
+  <data name="pictureBoxQuad.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
+    <value>NoControl</value>
+  </data>
+  <data name="pictureBoxQuad.Location" type="System.Drawing.Point, System.Drawing">
+    <value>112, 140</value>
+  </data>
+  <data name="pictureBoxQuad.Size" type="System.Drawing.Size, System.Drawing">
+    <value>190, 190</value>
+  </data>
+  <data name="pictureBoxQuad.SizeMode" type="System.Windows.Forms.PictureBoxSizeMode, System.Windows.Forms">
+    <value>Zoom</value>
+  </data>
+  <data name="pictureBoxQuad.TabIndex" type="System.Int32, mscorlib">
+    <value>4</value>
+  </data>
+  <data name="&gt;&gt;pictureBoxQuad.Name" xml:space="preserve">
+    <value>pictureBoxQuad</value>
+  </data>
+  <data name="&gt;&gt;pictureBoxQuad.Type" xml:space="preserve">
+    <value>System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </data>
+  <data name="&gt;&gt;pictureBoxQuad.Parent" xml:space="preserve">
+    <value>tabArducopter</value>
+  </data>
+  <data name="&gt;&gt;pictureBoxQuad.ZOrder" xml:space="preserve">
+    <value>5</value>
+  </data>
   <data name="Gservoloc.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
     <value>Zoom</value>
   </data>
@@ -5113,9 +5200,6 @@ will work with hexa's etc</value>
   <data name="&gt;&gt;Gservoloc.ZOrder" xml:space="preserve">
     <value>35</value>
   </data>
-  <metadata name="toolTip1.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
-    <value>214, 17</value>
-  </metadata>
   <metadata name="$this.Localizable" type="System.Boolean, mscorlib, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089">
     <value>True</value>
   </metadata>
@@ -5128,18 +5212,18 @@ will work with hexa's etc</value>
   <data name="$this.Text" xml:space="preserve">
     <value>APMSetup</value>
   </data>
-  <data name="&gt;&gt;currentStateBindingSource.Name" xml:space="preserve">
-    <value>currentStateBindingSource</value>
-  </data>
-  <data name="&gt;&gt;currentStateBindingSource.Type" xml:space="preserve">
-    <value>System.Windows.Forms.BindingSource, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
-  </data>
   <data name="&gt;&gt;toolTip1.Name" xml:space="preserve">
     <value>toolTip1</value>
   </data>
   <data name="&gt;&gt;toolTip1.Type" xml:space="preserve">
     <value>System.Windows.Forms.ToolTip, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
   </data>
+  <data name="&gt;&gt;currentStateBindingSource.Name" xml:space="preserve">
+    <value>currentStateBindingSource</value>
+  </data>
+  <data name="&gt;&gt;currentStateBindingSource.Type" xml:space="preserve">
+    <value>System.Windows.Forms.BindingSource, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </data>
   <data name="&gt;&gt;$this.Name" xml:space="preserve">
     <value>Setup</value>
   </data>

Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application

@@ -11,7 +11,7 @@
           <dsig:Transform Algorithm="urn:schemas-microsoft-com:HashTransforms.Identity" />
         </dsig:Transforms>
         <dsig:DigestMethod Algorithm="http://www.w3.org/2000/09/xmldsig#sha1" />
-        <dsig:DigestValue>+BB2J7z2to1UULmX82O0PxoC594=</dsig:DigestValue>
+        <dsig:DigestValue>/35AemMq933lmTElQtJvYj/CRU0=</dsig:DigestValue>
       </hash>
     </dependentAssembly>
   </dependency>

Tools/ArdupilotMegaPlanner/bin/Release/hud.html

@@ -19,7 +19,16 @@
     socket = new WebSocket(host);
     //log('WebSocket - status '+socket.readyState);
     socket.onopen    = function(msg){ socket.send("onopen");  alert("Welcome - status "+this.readyState); };
-    socket.onmessage = function(msg){ alert("Received: "+msg.data); };
+    socket.onmessage = function(msg){
+      var b1 = msg.data.indexOf(",",0);
+      var b2 = msg.data.indexOf(",",b1+1);
+      
+      roll = parseFloat( msg.data.substr(0,b1));
+      pitch = parseFloat( msg.data.substr(b1+1,b2 - b1));
+      yaw = parseFloat( msg.data.substr(b2+1));
+      
+      //alert("Received: "+msg.data);
+    };
     socket.onerror   = function(msg){ alert("Error: "+msg.data); };
     socket.onclose   = function(msg){ alert("Disconnected - status "+this.readyState); };
     }
@@ -28,16 +37,15 @@
     alert("This browser doesnt support websockets");
     }
     
-
     function draw() {
 
-    pitch -= 1.2;
+    //pitch -= 1.2;
-    roll += .75;
+    //roll += .75;
 
-    if (pitch < -90)
+    //if (pitch < -90)
-	pitch = 90;
+	//pitch = 90;
-	if (roll > 180)
+	//if (roll > 180)
-	roll = -180;
+	//roll = -180;
 	
       var canvas = document.getElementById("canvas");
       if (canvas.getContext) {
@@ -157,7 +165,7 @@
 	  
 	  try {
 	  
-	  socket.send("test "+pitch+"\n");
+	  //socket.send("test "+pitch+"\n");
 	  } catch (ex){ }// alert(ex); }
 	  	
 	setTimeout ( "draw()", 33 );

Tools/ArdupilotMegaPlanner/georefimage.cs

@@ -234,7 +234,7 @@ namespace ArdupilotMega
             this.TXT_offsetseconds.Name = "TXT_offsetseconds";
             this.TXT_offsetseconds.Size = new System.Drawing.Size(100, 20);
             this.TXT_offsetseconds.TabIndex = 4;
-            this.TXT_offsetseconds.Text = "-86156";
+            this.TXT_offsetseconds.Text = "0";
             // 
             // BUT_doit
             // 

Tools/ArdupilotMegaPlanner/hud.html

@@ -19,7 +19,16 @@
     socket = new WebSocket(host);
     //log('WebSocket - status '+socket.readyState);
     socket.onopen    = function(msg){ socket.send("onopen");  alert("Welcome - status "+this.readyState); };
-    socket.onmessage = function(msg){ alert("Received: "+msg.data); };
+    socket.onmessage = function(msg){
+      var b1 = msg.data.indexOf(",",0);
+      var b2 = msg.data.indexOf(",",b1+1);
+      
+      roll = parseFloat( msg.data.substr(0,b1));
+      pitch = parseFloat( msg.data.substr(b1+1,b2 - b1));
+      yaw = parseFloat( msg.data.substr(b2+1));
+      
+      //alert("Received: "+msg.data);
+    };
     socket.onerror   = function(msg){ alert("Error: "+msg.data); };
     socket.onclose   = function(msg){ alert("Disconnected - status "+this.readyState); };
     }
@@ -28,16 +37,15 @@
     alert("This browser doesnt support websockets");
     }
     
-
     function draw() {
 
-    pitch -= 1.2;
+    //pitch -= 1.2;
-    roll += .75;
+    //roll += .75;
 
-    if (pitch < -90)
+    //if (pitch < -90)
-	pitch = 90;
+	//pitch = 90;
-	if (roll > 180)
+	//if (roll > 180)
-	roll = -180;
+	//roll = -180;
 	
       var canvas = document.getElementById("canvas");
       if (canvas.getContext) {
@@ -157,7 +165,7 @@
 	  
 	  try {
 	  
-	  socket.send("test "+pitch+"\n");
+	  //socket.send("test "+pitch+"\n");
 	  } catch (ex){ }// alert(ex); }
 	  	
 	setTimeout ( "draw()", 33 );