diff --git a/Tools/ArdupilotMegaPlanner/Antenna/Maestro.cs b/Tools/ArdupilotMegaPlanner/Antenna/Maestro.cs index 9cdd6a7a20..75620862fb 100644 --- a/Tools/ArdupilotMegaPlanner/Antenna/Maestro.cs +++ b/Tools/ArdupilotMegaPlanner/Antenna/Maestro.cs @@ -110,7 +110,7 @@ namespace ArdupilotMega.Antenna // conver the angle into a 0-255 value byte target = (byte)((((PointAtAngle / range) * 2.0) * 127 + centerpos) * _panreverse); - Console.WriteLine("P " + Angle + " " + target + " " + PointAtAngle); + // Console.WriteLine("P " + Angle + " " + target + " " + PointAtAngle); var buffer = new byte[] { 0xff, PanAddress, target }; ComPort.Write(buffer, 0, buffer.Length); diff --git a/Tools/ArdupilotMegaPlanner/Antenna/Tracker.Designer.cs b/Tools/ArdupilotMegaPlanner/Antenna/Tracker.Designer.cs index ef9c5d36ea..a4e03e25b2 100644 --- a/Tools/ArdupilotMegaPlanner/Antenna/Tracker.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/Antenna/Tracker.Designer.cs @@ -55,6 +55,7 @@ this.BUT_connect = new ArdupilotMega.Controls.MyButton(); this.LBL_pantrim = new System.Windows.Forms.Label(); this.LBL_tilttrim = new System.Windows.Forms.Label(); + this.BUT_find = new ArdupilotMega.Controls.MyButton(); ((System.ComponentModel.ISupportInitialize)(this.TRK_pantrim)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.TRK_tilttrim)).BeginInit(); this.SuspendLayout(); @@ -222,10 +223,18 @@ resources.ApplyResources(this.LBL_tilttrim, "LBL_tilttrim"); this.LBL_tilttrim.Name = "LBL_tilttrim"; // + // BUT_find + // + resources.ApplyResources(this.BUT_find, "BUT_find"); + this.BUT_find.Name = "BUT_find"; + this.BUT_find.UseVisualStyleBackColor = true; + this.BUT_find.Click += new System.EventHandler(this.BUT_find_Click); + // // Tracker // resources.ApplyResources(this, "$this"); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; + this.Controls.Add(this.BUT_find); this.Controls.Add(this.LBL_tilttrim); this.Controls.Add(this.LBL_pantrim); this.Controls.Add(this.label12); @@ -288,5 +297,6 @@ private System.Windows.Forms.Label label12; private System.Windows.Forms.Label LBL_pantrim; private System.Windows.Forms.Label LBL_tilttrim; + private Controls.MyButton BUT_find; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Antenna/Tracker.cs b/Tools/ArdupilotMegaPlanner/Antenna/Tracker.cs index 3da8420be4..607e93114c 100644 --- a/Tools/ArdupilotMegaPlanner/Antenna/Tracker.cs +++ b/Tools/ArdupilotMegaPlanner/Antenna/Tracker.cs @@ -224,5 +224,81 @@ namespace ArdupilotMega.Antenna { saveconfig(); } + + private void BUT_find_Click(object sender, EventArgs e) + { + System.Threading.ThreadPool.QueueUserWorkItem(tm1_Tick); + } + + void tm1_Tick(object item) + { + + float snr = MainV2.cs.localsnrdb; + float best = snr; + + float tilt = 0; + float pan = 0; + + this.Invoke((MethodInvoker)delegate + { + tilt = TRK_tilttrim.Value; + pan = TRK_pantrim.Value; + }); + + + // scan entire range withing 30 degrees + float ans = checkpos((pan - float.Parse(TXT_panrange.Text) / 4), (pan + float.Parse(TXT_panrange.Text) / 4) - 1, 30); + + // scan new range within 30 - little overlap + ans = checkpos((-30 + ans), (30 + ans), 5); + + // scan new range + ans = checkpos((-5 + ans), (5 + ans), 1); + + setpan(ans); + + + } + + void setpan(float no) + { + this.Invoke((MethodInvoker)delegate + { + try + { + TRK_pantrim.Value = (int)no; + TRK_pantrim_Scroll(null, null); + } + catch { return; } + }); + } + + float checkpos(float start, float end,float scale) + { + float lastsnr = 0; + float best = 0; + + setpan(start); + + System.Threading.Thread.Sleep(4000); + + for (float n = start; n < end; n += scale) + { + setpan(n); + + System.Threading.Thread.Sleep(2000); + + Console.WriteLine("Angle " + n + " snr " + MainV2.cs.localsnrdb); + + if (MainV2.cs.localsnrdb > lastsnr) + { + + best = n; + lastsnr = MainV2.cs.localsnrdb; + } + } + + return best; + } } } diff --git a/Tools/ArdupilotMegaPlanner/Antenna/Tracker.resx b/Tools/ArdupilotMegaPlanner/Antenna/Tracker.resx index 74e13663a7..555781dc7a 100644 --- a/Tools/ArdupilotMegaPlanner/Antenna/Tracker.resx +++ b/Tools/ArdupilotMegaPlanner/Antenna/Tracker.resx @@ -147,7 +147,7 @@ $this - 25 + 26 True @@ -174,7 +174,7 @@ $this - 24 + 25 4800 @@ -219,7 +219,7 @@ $this - 21 + 22 210, 10 @@ -240,7 +240,7 @@ $this - 23 + 24 153, 81 @@ -261,7 +261,7 @@ $this - 20 + 21 83, 81 @@ -285,7 +285,7 @@ $this - 19 + 20 True @@ -312,7 +312,7 @@ $this - 18 + 19 True @@ -339,7 +339,7 @@ $this - 17 + 18 True @@ -366,7 +366,7 @@ $this - 13 + 14 True @@ -393,7 +393,7 @@ $this - 14 + 15 83, 157 @@ -417,7 +417,7 @@ $this - 15 + 16 153, 157 @@ -438,7 +438,7 @@ $this - 16 + 17 True @@ -465,7 +465,7 @@ $this - 12 + 13 True @@ -492,7 +492,7 @@ $this - 11 + 12 True @@ -519,7 +519,7 @@ $this - 10 + 11 True @@ -546,7 +546,7 @@ $this - 9 + 10 83, 107 @@ -570,7 +570,7 @@ $this - 8 + 9 83, 183 @@ -594,7 +594,7 @@ $this - 7 + 8 True @@ -621,7 +621,7 @@ $this - 6 + 7 True @@ -648,7 +648,7 @@ $this - 5 + 6 True @@ -675,7 +675,7 @@ $this - 3 + 4 True @@ -702,7 +702,7 @@ $this - 4 + 5 True @@ -732,7 +732,7 @@ $this - 2 + 3 476, 9 @@ -750,13 +750,13 @@ BUT_connect - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.1.4491.33622, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4605.34402, Culture=neutral, PublicKeyToken=null $this - 22 + 23 True @@ -783,7 +783,7 @@ $this - 1 + 2 True @@ -810,6 +810,30 @@ $this + 1 + + + 509, 51 + + + 71, 28 + + + 31 + + + Find Trim (3DR Radio) + + + BUT_find + + + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4605.34402, Culture=neutral, PublicKeyToken=null + + + $this + + 0 @@ -825,6 +849,6 @@ Tracker - ArdupilotMega.Controls.BackstageView.BackStageViewContentPanel, ArdupilotMegaPlanner, Version=1.1.4491.33622, Culture=neutral, PublicKeyToken=null + System.Windows.Forms.UserControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Arduino/ArduinoDetect.cs b/Tools/ArdupilotMegaPlanner/Arduino/ArduinoDetect.cs index 46b3aa60a6..6c0199befa 100644 --- a/Tools/ArdupilotMegaPlanner/Arduino/ArduinoDetect.cs +++ b/Tools/ArdupilotMegaPlanner/Arduino/ArduinoDetect.cs @@ -236,11 +236,11 @@ namespace ArdupilotMega.Arduino AP_PARAM_GROUP }; - static string[] type_names = new string[] { + internal static string[] type_names = new string[] { "NONE", "INT8", "INT16", "INT32", "FLOAT", "VECTOR3F", "VECTOR6F","MATRIX6F", "GROUP" }; - static byte type_size(ap_var_type type) + internal static byte type_size(ap_var_type type) { switch (type) { case ap_var_type.AP_PARAM_NONE: @@ -369,6 +369,50 @@ namespace ArdupilotMega.Arduino } } } + + if (buffer[0] == 'P' && buffer[1] == 'A' && buffer[2] == 6) // ap param + { + int pos = 4; + byte key = 0; + while (pos < (1024 * 4)) + { + key = buffer[pos]; + pos++; + + if (key == 0xff) + { + log.InfoFormat("end sentinal at {0}", pos - 1); + break; + } + + int type = buffer[pos] & 0x3f; // 6 bits + + uint group = BitConverter.ToUInt32(buffer, pos);//((byte)(buffer[pos]) >> 6) | ((byte)(buffer[pos + 1]) << 8) | ((byte)(buffer[pos + 2]) << 16); // 18 bits + + group = (group >> 6) & 0x3ffff; + pos++; + pos++; + pos++; + + int size = ArduinoDetect.type_size((ArduinoDetect.ap_var_type)Enum.Parse(typeof(ArduinoDetect.ap_var_type), type.ToString())); + + Console.Write("{0:X4}: type {1} ({2}) key {3} group_element {4} size {5} value ", pos - 4, type, ArduinoDetect.type_names[type], key, group, size); + + if (key == 0) + { + //Array.Reverse(buffer, pos, 2); + return BitConverter.ToUInt16(buffer, pos); + } + + + for (int i = 0; i < size; i++) + { + Console.Write(" {0:X2}", buffer[pos]); + pos++; + } + Console.WriteLine(); + } + } } return -1; } diff --git a/Tools/ArdupilotMegaPlanner/Comms/CommsSerialInterface.cs b/Tools/ArdupilotMegaPlanner/Comms/CommsSerialInterface.cs index 49d17b59f7..0bcdd7449d 100644 --- a/Tools/ArdupilotMegaPlanner/Comms/CommsSerialInterface.cs +++ b/Tools/ArdupilotMegaPlanner/Comms/CommsSerialInterface.cs @@ -30,7 +30,7 @@ namespace ArdupilotMega.Comms void toggleDTR(); // Properties - //Stream BaseStream { get; } + Stream BaseStream { get; } int BaudRate { get; set; } //bool BreakState { get; set; } int BytesToRead { get; } diff --git a/Tools/ArdupilotMegaPlanner/Comms/CommsTCPSerial.cs b/Tools/ArdupilotMegaPlanner/Comms/CommsTCPSerial.cs index 30e10bc527..8a6ee0729f 100644 --- a/Tools/ArdupilotMegaPlanner/Comms/CommsTCPSerial.cs +++ b/Tools/ArdupilotMegaPlanner/Comms/CommsTCPSerial.cs @@ -16,15 +16,15 @@ namespace ArdupilotMega.Comms { private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); TcpClient client = new TcpClient(); + BufferedStream clientbuf; IPEndPoint RemoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0); - byte[] rbuffer = new byte[0]; - int rbufferread = 0; int retrys = 3; public int WriteBufferSize { get; set; } public int WriteTimeout { get; set; } public bool RtsEnable { get; set; } + public Stream BaseStream { get { return this.BaseStream; } } ~TcpSerial() { @@ -63,7 +63,7 @@ namespace ArdupilotMega.Comms public int BytesToRead { - get { return client.Available + rbuffer.Length - rbufferread; } + get { /*Console.WriteLine(DateTime.Now.Millisecond + " tcp btr " + (client.Available + rbuffer.Length - rbufferread));*/ return (int)client.Available; } } public int BytesToWrite { get { return 0; } } @@ -95,13 +95,16 @@ namespace ArdupilotMega.Comms if (ArdupilotMega.MainV2.config["TCP_host"] != null) host = ArdupilotMega.MainV2.config["TCP_host"].ToString(); - if (System.Windows.Forms.DialogResult.Cancel == ArdupilotMega.Common.InputBox("remote host", "Enter host name/ip (ensure remote end is already started)", ref host)) + if (!MainV2.MONO) { - throw new Exception("Canceled by request"); - } - if (System.Windows.Forms.DialogResult.Cancel == ArdupilotMega.Common.InputBox("remote Port", "Enter remote port", ref dest)) - { - throw new Exception("Canceled by request"); + if (System.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"); + } + if (System.Windows.Forms.DialogResult.Cancel == ArdupilotMega.Common.InputBox("remote Port", "Enter remote port", ref dest)) + { + throw new Exception("Canceled by request"); + } } Port = dest; @@ -114,6 +117,8 @@ namespace ArdupilotMega.Comms client.NoDelay = true; client.Client.NoDelay = true; + clientbuf = new BufferedStream(client.GetStream()); + VerifyConnected(); return; @@ -132,7 +137,9 @@ namespace ArdupilotMega.Comms // this should only happen if we have established a connection in the first place if (client != null && retrys > 0) { + log.Info("tcp reconnect"); client.Connect(ArdupilotMega.MainV2.config["TCP_host"].ToString(), int.Parse(ArdupilotMega.MainV2.config["TCP_port"].ToString())); + clientbuf = new BufferedStream(client.GetStream()); retrys--; } @@ -147,7 +154,14 @@ namespace ArdupilotMega.Comms { if (length < 1) { return 0; } - return client.Client.Receive(readto, offset, length, SocketFlags.None); + return client.Client.Receive(readto, offset, length, SocketFlags.None); +/* + byte[] temp = new byte[length]; + clientbuf.Read(temp, 0, length); + + temp.CopyTo(readto, offset); + + return length;*/ } catch { throw new Exception("Socket Closed"); } } @@ -212,7 +226,7 @@ namespace ArdupilotMega.Comms public void DiscardInBuffer() { VerifyConnected(); - int size = client.Available; + int size = (int)client.Available; byte[] crap = new byte[size]; log.InfoFormat("TcpSerial DiscardInBuffer {0}",size); Read(crap, 0, size); diff --git a/Tools/ArdupilotMegaPlanner/Comms/CommsUdpSerial.cs b/Tools/ArdupilotMegaPlanner/Comms/CommsUdpSerial.cs index 5ab324bde1..19feae4e63 100644 --- a/Tools/ArdupilotMegaPlanner/Comms/CommsUdpSerial.cs +++ b/Tools/ArdupilotMegaPlanner/Comms/CommsUdpSerial.cs @@ -21,6 +21,7 @@ namespace ArdupilotMega.Comms public int WriteBufferSize { get; set; } public int WriteTimeout { get; set; } public bool RtsEnable { get; set; } + public Stream BaseStream { get { return this.BaseStream; } } ~UdpSerial() { diff --git a/Tools/ArdupilotMegaPlanner/Controls/ConnectionControl.Designer.cs b/Tools/ArdupilotMegaPlanner/Controls/ConnectionControl.Designer.cs index 480bc602ea..a5ab90d018 100644 --- a/Tools/ArdupilotMegaPlanner/Controls/ConnectionControl.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/Controls/ConnectionControl.Designer.cs @@ -46,7 +46,7 @@ "38400", "57600", "115200"}); - this.cmb_Baud.Location = new System.Drawing.Point(130, 12); + this.cmb_Baud.Location = new System.Drawing.Point(130, 4); this.cmb_Baud.Name = "cmb_Baud"; this.cmb_Baud.Size = new System.Drawing.Size(70, 21); this.cmb_Baud.TabIndex = 0; @@ -54,7 +54,7 @@ // cmb_ConnectionType // this.cmb_ConnectionType.FormattingEnabled = true; - this.cmb_ConnectionType.Location = new System.Drawing.Point(79, 39); + this.cmb_ConnectionType.Location = new System.Drawing.Point(79, 31); this.cmb_ConnectionType.Name = "cmb_ConnectionType"; this.cmb_ConnectionType.Size = new System.Drawing.Size(121, 21); this.cmb_ConnectionType.TabIndex = 1; @@ -63,7 +63,7 @@ // cmb_Connection // this.cmb_Connection.FormattingEnabled = true; - this.cmb_Connection.Location = new System.Drawing.Point(3, 12); + this.cmb_Connection.Location = new System.Drawing.Point(3, 4); this.cmb_Connection.Name = "cmb_Connection"; this.cmb_Connection.Size = new System.Drawing.Size(121, 21); this.cmb_Connection.TabIndex = 2; @@ -72,7 +72,7 @@ // this.linkLabel1.AutoSize = true; this.linkLabel1.Image = global::ArdupilotMega.Properties.Resources.bg; - this.linkLabel1.Location = new System.Drawing.Point(3, 55); + this.linkLabel1.Location = new System.Drawing.Point(3, 44); this.linkLabel1.Name = "linkLabel1"; this.linkLabel1.Size = new System.Drawing.Size(63, 13); this.linkLabel1.TabIndex = 3; @@ -90,7 +90,7 @@ this.Controls.Add(this.cmb_ConnectionType); this.Controls.Add(this.cmb_Baud); this.Name = "ConnectionControl"; - this.Size = new System.Drawing.Size(230, 76); + this.Size = new System.Drawing.Size(230, 60); this.MouseClick += new System.Windows.Forms.MouseEventHandler(this.ConnectionControl_MouseClick); this.ResumeLayout(false); this.PerformLayout(); diff --git a/Tools/ArdupilotMegaPlanner/Controls/HUD.cs b/Tools/ArdupilotMegaPlanner/Controls/HUD.cs index 3312fe53e1..88d7e48fd8 100644 --- a/Tools/ArdupilotMegaPlanner/Controls/HUD.cs +++ b/Tools/ArdupilotMegaPlanner/Controls/HUD.cs @@ -150,6 +150,9 @@ namespace ArdupilotMega.Controls [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] public DateTime datetime { get { return _datetime; } set { if (_datetime != value) { _datetime = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float groundalt { get; set; } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] public int status { get; set; } @@ -1245,6 +1248,8 @@ namespace ArdupilotMega.Controls greenPen.Color = Color.FromArgb(255, greenPen.Color); } + bool ground = false; + for (int a = (int)start; a <= (_alt + viewrange / 2); a += 1) { if (a == Math.Round(_targetalt) && _targetalt != 0) @@ -1252,12 +1257,21 @@ namespace ArdupilotMega.Controls greenPen.Width = 6; graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top - space * (a - start), scrollbg.Left + scrollbg.Width, scrollbg.Top - space * (a - start)); } + + + // ground doesnt appear if we are not in view or below ground level + if (a == Math.Round(groundalt) && groundalt != 0 && ground == false) + { + graphicsObject.FillRectangle(new SolidBrush(Color.FromArgb(100,Color.BurlyWood)), new RectangleF(scrollbg.Left, scrollbg.Top - space * (a - start), scrollbg.Width, (space * (a - start)))); + } + if (a % 5 == 0) { //Console.WriteLine(a + " " + scrollbg.Left + " " + (scrollbg.Top - space * (a - start)) + " " + (scrollbg.Left + 20) + " " + (scrollbg.Top - space * (a - start))); graphicsObject.DrawLine(whitePen, scrollbg.Left, scrollbg.Top - space * (a - start), scrollbg.Left + 10, scrollbg.Top - space * (a - start)); drawstring(graphicsObject, a.ToString().PadLeft(5), font, fontsize, whiteBrush, scrollbg.Left + 0 + (int)(0 * fontoffset), scrollbg.Top - space * (a - start) - 6 - fontoffset); } + } greenPen.Width = 4; diff --git a/Tools/ArdupilotMegaPlanner/Controls/MainSwitcher.cs b/Tools/ArdupilotMegaPlanner/Controls/MainSwitcher.cs index f298cb9e82..a3425f3129 100644 --- a/Tools/ArdupilotMegaPlanner/Controls/MainSwitcher.cs +++ b/Tools/ArdupilotMegaPlanner/Controls/MainSwitcher.cs @@ -13,7 +13,7 @@ namespace ArdupilotMega.Controls { public partial class MainSwitcher : UserControl { - List screens = new List(); + public List screens = new List(); Screen current; public MainSwitcher() diff --git a/Tools/ArdupilotMegaPlanner/Controls/MyLabel.cs b/Tools/ArdupilotMegaPlanner/Controls/MyLabel.cs index a3ff19f0b3..7d10f2f487 100644 --- a/Tools/ArdupilotMegaPlanner/Controls/MyLabel.cs +++ b/Tools/ArdupilotMegaPlanner/Controls/MyLabel.cs @@ -18,11 +18,18 @@ namespace ArdupilotMega.Controls int noofchars = 0; bool autosize = false; + SolidBrush s = new SolidBrush(Color.White); + SolidBrush b = new SolidBrush(Color.Black); + + StringFormat stringFormat = new StringFormat(); + [System.ComponentModel.Browsable(true)] public bool resize { get { return autosize; } set { autosize = value; } } public MyLabel() { + stringFormat.Alignment = StringAlignment.Near; + stringFormat.LineAlignment = StringAlignment.Center; } public override string Text @@ -81,30 +88,45 @@ namespace ArdupilotMega.Controls base.OnVisibleChanged(e); } + public override Color BackColor + { + get + { + return base.BackColor; + } + set + { + base.BackColor = value; + b = new SolidBrush(value); + this.Invalidate(); + } + } - SolidBrush s = new SolidBrush(Color.White); - SolidBrush b = new SolidBrush(Color.Black); - StringFormat stringFormat = new StringFormat(); + public override System.Drawing.Color ForeColor + { + get + { + return base.ForeColor; + } + set + { + base.ForeColor = value; + s = new SolidBrush(value); + this.Invalidate(); + } + } protected override void OnPaint(PaintEventArgs e) { // TextRenderer.DrawText(e.Graphics, label, this.Font, new Point(0, 0), ForeColor); - stringFormat.Alignment = StringAlignment.Near; - stringFormat.LineAlignment = StringAlignment.Center; - - s = new SolidBrush(ForeColor); - e.Graphics.DrawString(label, this.Font, s, new PointF(0, this.Height / 2.0f), stringFormat); - } protected override void OnPaintBackground(PaintEventArgs pevent) { - b = new SolidBrush(BackColor); - pevent.Graphics.FillRectangle(b, this.ClientRectangle); base.OnPaintBackground(pevent); diff --git a/Tools/ArdupilotMegaPlanner/Controls/MyUserControl.cs b/Tools/ArdupilotMegaPlanner/Controls/MyUserControl.cs index d668d91bf1..5f28efa932 100644 --- a/Tools/ArdupilotMegaPlanner/Controls/MyUserControl.cs +++ b/Tools/ArdupilotMegaPlanner/Controls/MyUserControl.cs @@ -13,7 +13,7 @@ namespace System.Windows.Forms { /// /// implement an on closing event to tidy up enviroment. - /// Using preedefined refrence as can easerly change betwen form and user control this way. + /// Using preedefined refrence as can easerly change between form and user control this way. /// public event FormClosingEventHandler FormClosing; diff --git a/Tools/ArdupilotMegaPlanner/Controls/ProgressReporterDialogue.designer.cs b/Tools/ArdupilotMegaPlanner/Controls/ProgressReporterDialogue.designer.cs index cf9cbc46bb..d698147448 100644 --- a/Tools/ArdupilotMegaPlanner/Controls/ProgressReporterDialogue.designer.cs +++ b/Tools/ArdupilotMegaPlanner/Controls/ProgressReporterDialogue.designer.cs @@ -109,6 +109,7 @@ namespace ArdupilotMega.Controls // timer1 // this.timer1.Enabled = true; + this.timer1.Interval = 200; this.timer1.Tick += new System.EventHandler(this.timer1_Tick); // // ProgressReporterDialogue diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index 7c38f54a3a..10c4d93494 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -202,7 +202,10 @@ namespace ArdupilotMega public ushort rcoverridech7 { get; set; } public ushort rcoverridech8 { get; set; } + + public float HomeAlt { get { return (float)HomeLocation.Alt; } set { } } internal PointLatLngAlt HomeLocation = new PointLatLngAlt(); + public float DistToMAV { get diff --git a/Tools/ArdupilotMegaPlanner/FollowMe.cs b/Tools/ArdupilotMegaPlanner/FollowMe.cs index 7365059476..e92baf2c22 100644 --- a/Tools/ArdupilotMegaPlanner/FollowMe.cs +++ b/Tools/ArdupilotMegaPlanner/FollowMe.cs @@ -51,7 +51,7 @@ namespace ArdupilotMega } catch {CustomMessageBox.Show("Invalid BaudRate"); return;} try { comPort.Open(); - } catch {CustomMessageBox.Show("Error Connecting\nif using com0com please rename the ports to COM??"); return;} + } catch (Exception ex) {CustomMessageBox.Show("Error Connecting\nif using com0com please rename the ports to COM??\n" + ex.ToString()); return;} string alt = "100"; @@ -159,7 +159,7 @@ namespace ArdupilotMega { MainV2.giveComport = true; - MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2); + MainV2.comPort.setGuidedModeWP(gotohere); GCSViews.FlightData.GuidedModeWP = new PointLatLngAlt(gotohere); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs index 1350bf6df0..4b5e0bd23e 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs @@ -404,18 +404,19 @@ namespace ArdupilotMega.GCSViews } catch (Exception ex) { lbl_status.Text = "Failed download"; CustomMessageBox.Show("Failed to download new firmware : " + ex.ToString()); return; } - System.Threading.ThreadPool.QueueUserWorkItem(fwtype, temp); + System.Threading.ThreadPool.QueueUserWorkItem(apmtype, temp.name + "!" + board); UploadFlash(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex", board); } - void fwtype(object temp) + void apmtype(object temp) { - software sw = (software)temp; try { // Create a request using a URL that can receive a post. - WebRequest request = WebRequest.Create("http://vps.oborne.me/" + sw.name); + HttpWebRequest request = (HttpWebRequest)HttpWebRequest.Create("http://vps.oborne.me/axs/ax.pl?" + (string)temp); + //request.AllowAutoRedirect = true; + request.UserAgent = MainV2.instance.Text + " (res" + MainV2.instance.Width + "x" + MainV2.instance.Height + "; " + Environment.OSVersion.VersionString + ")"; request.Timeout = 10000; // Set the Method property of the request to POST. request.Method = "GET"; diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs index e36d1e220c..5c7dac90c5 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs @@ -12,7 +12,9 @@ System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle(); this.contextMenuStrip1 = new System.Windows.Forms.ContextMenuStrip(this.components); this.goHereToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.flyToHereAltToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.pointCameraHereToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.flightPlannerToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.MainH = new System.Windows.Forms.SplitContainer(); this.SubMainLeft = new System.Windows.Forms.SplitContainer(); this.hud1 = new ArdupilotMega.Controls.HUD(); @@ -110,7 +112,9 @@ // this.contextMenuStrip1.Items.AddRange(new System.Windows.Forms.ToolStripItem[] { this.goHereToolStripMenuItem, - this.pointCameraHereToolStripMenuItem}); + this.flyToHereAltToolStripMenuItem, + this.pointCameraHereToolStripMenuItem, + this.flightPlannerToolStripMenuItem}); this.contextMenuStrip1.Name = "contextMenuStrip1"; resources.ApplyResources(this.contextMenuStrip1, "contextMenuStrip1"); // @@ -120,12 +124,24 @@ resources.ApplyResources(this.goHereToolStripMenuItem, "goHereToolStripMenuItem"); this.goHereToolStripMenuItem.Click += new System.EventHandler(this.goHereToolStripMenuItem_Click); // + // flyToHereAltToolStripMenuItem + // + this.flyToHereAltToolStripMenuItem.Name = "flyToHereAltToolStripMenuItem"; + resources.ApplyResources(this.flyToHereAltToolStripMenuItem, "flyToHereAltToolStripMenuItem"); + this.flyToHereAltToolStripMenuItem.Click += new System.EventHandler(this.flyToHereAltToolStripMenuItem_Click); + // // pointCameraHereToolStripMenuItem // this.pointCameraHereToolStripMenuItem.Name = "pointCameraHereToolStripMenuItem"; resources.ApplyResources(this.pointCameraHereToolStripMenuItem, "pointCameraHereToolStripMenuItem"); this.pointCameraHereToolStripMenuItem.Click += new System.EventHandler(this.pointCameraHereToolStripMenuItem_Click); // + // flightPlannerToolStripMenuItem + // + this.flightPlannerToolStripMenuItem.Name = "flightPlannerToolStripMenuItem"; + resources.ApplyResources(this.flightPlannerToolStripMenuItem, "flightPlannerToolStripMenuItem"); + this.flightPlannerToolStripMenuItem.Click += new System.EventHandler(this.flightPlannerToolStripMenuItem_Click); + // // MainH // this.MainH.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle; @@ -173,6 +189,7 @@ this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("disttowp", this.bindingSource1, "wp_dist", true)); this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("gpsfix", this.bindingSource1, "gpsstatus", true)); this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("gpshdop", this.bindingSource1, "gpshdop", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("groundalt", this.bindingSource1, "HomeAlt", true)); this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("groundcourse", this.bindingSource1, "groundcourse", true)); this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("groundspeed", this.bindingSource1, "groundspeed", true)); this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("heading", this.bindingSource1, "yaw", true)); @@ -195,6 +212,7 @@ resources.ApplyResources(this.hud1, "hud1"); this.hud1.gpsfix = 0F; this.hud1.gpshdop = 0F; + this.hud1.groundalt = 0F; this.hud1.groundcourse = 0F; this.hud1.groundspeed = 0F; this.hud1.heading = 0F; @@ -1040,6 +1058,7 @@ // // splitContainer1.Panel2 // + this.splitContainer1.Panel2.ContextMenuStrip = this.contextMenuStrip1; this.splitContainer1.Panel2.Controls.Add(this.lbl_winddir); this.splitContainer1.Panel2.Controls.Add(this.lbl_windvel); this.splitContainer1.Panel2.Controls.Add(this.lbl_hdop); @@ -1346,5 +1365,7 @@ private Controls.QuickView quickView4; private Controls.QuickView quickView6; private Controls.QuickView quickView5; + private System.Windows.Forms.ToolStripMenuItem flyToHereAltToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem flightPlannerToolStripMenuItem; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index cb378419b9..def244e843 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -227,6 +227,11 @@ namespace ArdupilotMega.GCSViews hud1.Enabled = true; hud1.Dock = DockStyle.Fill; + + foreach (Control ctl in splitContainer1.Panel2.Controls) + { + ctl.Visible = true; + } } public void Deactivate() @@ -312,7 +317,7 @@ namespace ArdupilotMega.GCSViews if (MainV2.giveComport == true) { - System.Threading.Thread.Sleep(20); + System.Threading.Thread.Sleep(100); continue; } if (!comPort.BaseStream.IsOpen) @@ -571,7 +576,7 @@ namespace ArdupilotMega.GCSViews if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane) { - routes.Markers[0] = (new GMapMarkerPlane(currentloc, MainV2.cs.yaw, MainV2.cs.groundcourse, MainV2.cs.nav_bearing, MainV2.cs.target_bearing, gMapControl1)); + routes.Markers[0] = (new GMapMarkerPlane(currentloc, MainV2.cs.yaw, MainV2.cs.groundcourse, MainV2.cs.nav_bearing, MainV2.cs.target_bearing, gMapControl1) { ToolTipText = MainV2.cs.alt.ToString("0"), ToolTipMode = MarkerTooltipMode.Always }); } else if (MainV2.cs.firmware == MainV2.Firmwares.ArduRover) { @@ -1040,30 +1045,12 @@ namespace ArdupilotMega.GCSViews return; } - string alt = "100"; - - if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2) + if (GuidedModeWP.Alt == 0) { - alt = (10 * MainV2.cs.multiplierdist).ToString("0"); - } - else - { - alt = (100 * MainV2.cs.multiplierdist).ToString("0"); - } + flyToHereAltToolStripMenuItem_Click(null, null); - if (MainV2.config.ContainsKey("guided_alt")) - alt = MainV2.config["guided_alt"].ToString(); - - if (DialogResult.Cancel == Common.InputBox("Enter Alt", "Enter Guided Mode Alt", ref alt)) - return; - - MainV2.config["guided_alt"] = alt; - - int intalt = (int)(100 * MainV2.cs.multiplierdist); - if (!int.TryParse(alt, out intalt)) - { - CustomMessageBox.Show("Bad Alt"); - return; + if (GuidedModeWP.Alt == 0) + return; } if (gotolocation.Lat == 0 || gotolocation.Lng == 0) @@ -1075,19 +1062,15 @@ namespace ArdupilotMega.GCSViews Locationwp gotohere = new Locationwp(); gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; - gotohere.alt = (float)(intalt / MainV2.cs.multiplierdist); // back to m + gotohere.alt = (float)(GuidedModeWP.Alt); // back to m gotohere.lat = (float)(gotolocation.Lat); gotohere.lng = (float)(gotolocation.Lng); try { - MainV2.giveComport = true; - - MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2); + MainV2.comPort.setGuidedModeWP(gotohere); GuidedModeWP = new PointLatLngAlt(gotohere.lat, gotohere.lng, gotohere.alt,"Guided Mode"); - - MainV2.giveComport = false; } catch (Exception ex) { MainV2.giveComport = false; CustomMessageBox.Show("Error sending command : " + ex.Message); } @@ -2150,5 +2133,98 @@ print 'Roll complete' ((Form)((CheckBox)sender).Parent).Close(); } } + + private void flyToHereAltToolStripMenuItem_Click(object sender, EventArgs e) + { + string alt = "100"; + + if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2) + { + alt = (10 * MainV2.cs.multiplierdist).ToString("0"); + } + else + { + alt = (100 * MainV2.cs.multiplierdist).ToString("0"); + } + + if (MainV2.config.ContainsKey("guided_alt")) + alt = MainV2.config["guided_alt"].ToString(); + + if (DialogResult.Cancel == Common.InputBox("Enter Alt", "Enter Guided Mode Alt", ref alt)) + return; + + MainV2.config["guided_alt"] = alt; + + int intalt = (int)(100 * MainV2.cs.multiplierdist); + if (!int.TryParse(alt, out intalt)) + { + CustomMessageBox.Show("Bad Alt"); + return; + } + + GuidedModeWP.Alt = intalt; + + if (MainV2.cs.mode == "Guided") + { + MainV2.comPort.setGuidedModeWP(new Locationwp() { alt = (float)GuidedModeWP.Alt, lat = (float)GuidedModeWP.Lat, lng = (float)GuidedModeWP.Lng }); + } + } + + private void flightPlannerToolStripMenuItem_Click(object sender, EventArgs e) + { + foreach (Control ctl in splitContainer1.Panel2.Controls) + { + ctl.Visible = false; + } + + foreach (MainSwitcher.Screen sc in MainV2.View.screens) + { + if (sc.Name == "FlightPlanner") + { + MyButton but = new MyButton() { Location = new Point(splitContainer1.Panel2.Width / 2, 0), Text = "Close" }; + but.Click += new EventHandler(but_Click); + + splitContainer1.Panel2.Controls.Add(but); + splitContainer1.Panel2.Controls.Add(sc.Control); + ThemeManager.ApplyThemeTo(sc.Control); + + sc.Control.Dock = DockStyle.Fill; + sc.Control.Visible = true; + + if (sc.Control is IActivate) + { + ((IActivate)(sc.Control)).Activate(); + } + + but.BringToFront(); + break; + } + } + } + + void but_Click(object sender, EventArgs e) + { + foreach (MainSwitcher.Screen sc in MainV2.View.screens) + { + if (sc.Name == "FlightPlanner") + { + splitContainer1.Panel2.Controls.Remove(sc.Control); + splitContainer1.Panel2.Controls.Remove((Control)sender); + sc.Control.Visible = false; + + if (sc.Control is IDeactivate) + { + ((IDeactivate)(sc.Control)).Deactivate(); + } + + break; + } + } + + foreach (Control ctl in splitContainer1.Panel2.Controls) + { + ctl.Visible = true; + } + } } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx index b11e0afb02..9a73daa0fa 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx @@ -127,14 +127,26 @@ Fly To Here + + 174, 22 + + + Fly To Here Alt + 174, 22 Point Camera Here + + 174, 22 + + + Flight Planner + - 175, 70 + 175, 92 contextMenuStrip1 @@ -226,7 +238,7 @@ hud1 - ArdupilotMega.Controls.HUD, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.HUD, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null SubMainLeft.Panel1 @@ -262,7 +274,7 @@ quickView6 - ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabQuick @@ -283,7 +295,7 @@ quickView5 - ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabQuick @@ -304,7 +316,7 @@ quickView4 - ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabQuick @@ -325,7 +337,7 @@ quickView3 - ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabQuick @@ -346,7 +358,7 @@ quickView2 - ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabQuick @@ -367,7 +379,7 @@ quickView1 - ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.QuickView, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabQuick @@ -421,7 +433,7 @@ BUT_script - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -454,7 +466,7 @@ BUT_joystick - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -484,7 +496,7 @@ BUT_quickmanual - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -514,7 +526,7 @@ BUT_quickrtl - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -544,7 +556,7 @@ BUT_quickauto - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -598,7 +610,7 @@ BUT_setwp - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -649,7 +661,7 @@ BUT_setmode - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -679,7 +691,7 @@ BUT_clear_track - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -730,7 +742,7 @@ BUT_Homealt - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -760,7 +772,7 @@ BUT_RAWSensor - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -790,7 +802,7 @@ BUTrestartmission - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -820,7 +832,7 @@ BUTactiondo - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabActions @@ -874,7 +886,7 @@ Gvspeed - AGaugeApp.AGauge, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + AGaugeApp.AGauge, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabGauges @@ -904,7 +916,7 @@ Gheading - ArdupilotMega.Controls.HSI, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.HSI, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabGauges @@ -934,7 +946,7 @@ Galt - AGaugeApp.AGauge, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + AGaugeApp.AGauge, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabGauges @@ -967,7 +979,7 @@ Gspeed - AGaugeApp.AGauge, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + AGaugeApp.AGauge, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabGauges @@ -1051,7 +1063,7 @@ lbl_playbackspeed - ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabTLogs @@ -1078,7 +1090,7 @@ lbl_logpercent - ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabTLogs @@ -1105,7 +1117,7 @@ NUM_playbackspeed - ArdupilotMega.Controls.MyTrackBar, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyTrackBar, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabTLogs @@ -1132,7 +1144,7 @@ BUT_log2kml - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabTLogs @@ -1186,7 +1198,7 @@ BUT_playlog - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabTLogs @@ -1213,7 +1225,7 @@ BUT_loadtelem - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null tabTLogs @@ -1399,7 +1411,7 @@ lbl_winddir - ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null splitContainer1.Panel2 @@ -1429,7 +1441,7 @@ lbl_windvel - ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null splitContainer1.Panel2 @@ -1462,7 +1474,7 @@ lbl_hdop - ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null splitContainer1.Panel2 @@ -1495,7 +1507,7 @@ lbl_sats - ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null splitContainer1.Panel2 @@ -1667,7 +1679,7 @@ gMapControl1 - ArdupilotMega.Controls.myGMAP, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.myGMAP, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null splitContainer1.Panel2 @@ -1730,7 +1742,7 @@ TXT_lat - ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null panel1 @@ -1787,7 +1799,7 @@ label1 - ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null panel1 @@ -1817,7 +1829,7 @@ TXT_long - ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null panel1 @@ -1847,7 +1859,7 @@ TXT_alt - ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null panel1 @@ -2048,7 +2060,7 @@ label6 - ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyLabel, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null $this @@ -2074,12 +2086,24 @@ System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + flyToHereAltToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + pointCameraHereToolStripMenuItem System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + flightPlannerToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + recordHudToAVIToolStripMenuItem @@ -2144,6 +2168,6 @@ FlightData - System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner10, Version=1.1.4602.32614, Culture=neutral, PublicKeyToken=null + System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner10, Version=1.1.4606.24956, Culture=neutral, PublicKeyToken=null \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs index 9a16bdec80..e5a0cae95e 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs @@ -86,14 +86,9 @@ this.TXT_mouselat = new System.Windows.Forms.TextBox(); this.comboBoxMapType = new System.Windows.Forms.ComboBox(); this.lbl_status = new System.Windows.Forms.Label(); - this.textBox1 = new System.Windows.Forms.TextBox(); this.panelWaypoints = new BSE.Windows.Forms.Panel(); this.splitter1 = new BSE.Windows.Forms.Splitter(); - this.BUT_loadkml = new ArdupilotMega.Controls.MyButton(); - this.BUT_zoomto = new ArdupilotMega.Controls.MyButton(); this.BUT_Camera = new ArdupilotMega.Controls.MyButton(); - this.BUT_grid = new ArdupilotMega.Controls.MyButton(); - this.BUT_Prefetch = new ArdupilotMega.Controls.MyButton(); this.button1 = new ArdupilotMega.Controls.MyButton(); this.BUT_Add = new ArdupilotMega.Controls.MyButton(); this.panelAction = new BSE.Windows.Forms.Panel(); @@ -104,6 +99,7 @@ this.MainMap = new ArdupilotMega.Controls.myGMAP(); this.contextMenuStrip1 = new System.Windows.Forms.ContextMenuStrip(this.components); this.deleteWPToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.setROIToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.loiterToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.loiterForeverToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.loitertimeToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); @@ -111,10 +107,8 @@ this.jumpToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.jumpstartToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.jumpwPToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); - this.createWpCircleToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.clearMissionToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.toolStripSeparator1 = new System.Windows.Forms.ToolStripSeparator(); - this.ContextMeasure = new System.Windows.Forms.ToolStripMenuItem(); - this.rotateMapToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.polygonToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.addPolygonPointToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.clearPolygonToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); @@ -126,7 +120,15 @@ this.setReturnLocationToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.loadFromFileToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.saveToFileToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); - this.clearMissionToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.autoWPToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.createWpCircleToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.gridToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.mapToolToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.ContextMeasure = new System.Windows.Forms.ToolStripMenuItem(); + this.rotateMapToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.zoomToToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.prefetchToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.kMLOverlayToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.trackBar1 = new ArdupilotMega.Controls.MyTrackBar(); this.label11 = new System.Windows.Forms.Label(); this.panelBASE = new System.Windows.Forms.Panel(); @@ -504,23 +506,15 @@ resources.ApplyResources(this.lbl_status, "lbl_status"); this.lbl_status.Name = "lbl_status"; // - // textBox1 - // - resources.ApplyResources(this.textBox1, "textBox1"); - this.textBox1.Name = "textBox1"; - // // panelWaypoints // this.panelWaypoints.AssociatedSplitter = this.splitter1; + resources.ApplyResources(this.panelWaypoints, "panelWaypoints"); this.panelWaypoints.BackColor = System.Drawing.Color.Transparent; this.panelWaypoints.CaptionFont = new System.Drawing.Font("Segoe UI", 11.75F, System.Drawing.FontStyle.Bold); this.panelWaypoints.CaptionHeight = 21; this.panelWaypoints.ColorScheme = BSE.Windows.Forms.ColorScheme.Custom; - this.panelWaypoints.Controls.Add(this.BUT_loadkml); - this.panelWaypoints.Controls.Add(this.BUT_zoomto); this.panelWaypoints.Controls.Add(this.BUT_Camera); - this.panelWaypoints.Controls.Add(this.BUT_grid); - this.panelWaypoints.Controls.Add(this.BUT_Prefetch); this.panelWaypoints.Controls.Add(this.CHK_altmode); this.panelWaypoints.Controls.Add(this.LBL_WPRad); this.panelWaypoints.Controls.Add(this.button1); @@ -546,7 +540,6 @@ this.panelWaypoints.CustomColors.ContentGradientBegin = System.Drawing.SystemColors.ButtonFace; this.panelWaypoints.CustomColors.ContentGradientEnd = System.Drawing.Color.FromArgb(((int)(((byte)(252)))), ((int)(((byte)(252)))), ((int)(((byte)(252))))); this.panelWaypoints.CustomColors.InnerBorderColor = System.Drawing.SystemColors.Window; - resources.ApplyResources(this.panelWaypoints, "panelWaypoints"); this.panelWaypoints.ForeColor = System.Drawing.SystemColors.ControlText; this.panelWaypoints.Image = null; this.panelWaypoints.LinearGradientMode = System.Drawing.Drawing2D.LinearGradientMode.Vertical; @@ -565,22 +558,6 @@ this.splitter1.Name = "splitter1"; this.splitter1.TabStop = false; // - // BUT_loadkml - // - resources.ApplyResources(this.BUT_loadkml, "BUT_loadkml"); - this.BUT_loadkml.Name = "BUT_loadkml"; - this.toolTip1.SetToolTip(this.BUT_loadkml, resources.GetString("BUT_loadkml.ToolTip")); - this.BUT_loadkml.UseVisualStyleBackColor = true; - this.BUT_loadkml.Click += new System.EventHandler(this.BUT_loadkml_Click); - // - // BUT_zoomto - // - resources.ApplyResources(this.BUT_zoomto, "BUT_zoomto"); - this.BUT_zoomto.Name = "BUT_zoomto"; - this.toolTip1.SetToolTip(this.BUT_zoomto, resources.GetString("BUT_zoomto.ToolTip")); - this.BUT_zoomto.UseVisualStyleBackColor = true; - this.BUT_zoomto.Click += new System.EventHandler(this.BUT_zoomto_Click); - // // BUT_Camera // resources.ApplyResources(this.BUT_Camera, "BUT_Camera"); @@ -589,22 +566,6 @@ this.BUT_Camera.UseVisualStyleBackColor = true; this.BUT_Camera.Click += new System.EventHandler(this.BUT_Camera_Click); // - // BUT_grid - // - resources.ApplyResources(this.BUT_grid, "BUT_grid"); - this.BUT_grid.Name = "BUT_grid"; - this.toolTip1.SetToolTip(this.BUT_grid, resources.GetString("BUT_grid.ToolTip")); - this.BUT_grid.UseVisualStyleBackColor = true; - this.BUT_grid.Click += new System.EventHandler(this.BUT_grid_Click); - // - // BUT_Prefetch - // - resources.ApplyResources(this.BUT_Prefetch, "BUT_Prefetch"); - this.BUT_Prefetch.Name = "BUT_Prefetch"; - this.toolTip1.SetToolTip(this.BUT_Prefetch, resources.GetString("BUT_Prefetch.ToolTip")); - this.BUT_Prefetch.UseVisualStyleBackColor = true; - this.BUT_Prefetch.Click += new System.EventHandler(this.BUT_Prefetch_Click); - // // button1 // resources.ApplyResources(this.button1, "button1"); @@ -628,7 +589,6 @@ this.panelAction.CaptionFont = new System.Drawing.Font("Segoe UI", 11.75F, System.Drawing.FontStyle.Bold); this.panelAction.CaptionHeight = 21; this.panelAction.ColorScheme = BSE.Windows.Forms.ColorScheme.Custom; - this.panelAction.Controls.Add(this.textBox1); this.panelAction.Controls.Add(this.panel5); this.panelAction.Controls.Add(this.panel1); this.panelAction.Controls.Add(this.panel2); @@ -720,15 +680,15 @@ // this.contextMenuStrip1.Items.AddRange(new System.Windows.Forms.ToolStripItem[] { this.deleteWPToolStripMenuItem, + this.setROIToolStripMenuItem, this.loiterToolStripMenuItem, this.jumpToolStripMenuItem, - this.createWpCircleToolStripMenuItem, + this.clearMissionToolStripMenuItem, this.toolStripSeparator1, - this.ContextMeasure, - this.rotateMapToolStripMenuItem, this.polygonToolStripMenuItem, this.geoFenceToolStripMenuItem, - this.clearMissionToolStripMenuItem}); + this.autoWPToolStripMenuItem, + this.mapToolToolStripMenuItem}); this.contextMenuStrip1.Name = "contextMenuStrip1"; resources.ApplyResources(this.contextMenuStrip1, "contextMenuStrip1"); // @@ -738,6 +698,12 @@ resources.ApplyResources(this.deleteWPToolStripMenuItem, "deleteWPToolStripMenuItem"); this.deleteWPToolStripMenuItem.Click += new System.EventHandler(this.deleteWPToolStripMenuItem_Click); // + // setROIToolStripMenuItem + // + this.setROIToolStripMenuItem.Name = "setROIToolStripMenuItem"; + resources.ApplyResources(this.setROIToolStripMenuItem, "setROIToolStripMenuItem"); + this.setROIToolStripMenuItem.Click += new System.EventHandler(this.setROIToolStripMenuItem_Click); + // // loiterToolStripMenuItem // this.loiterToolStripMenuItem.DropDownItems.AddRange(new System.Windows.Forms.ToolStripItem[] { @@ -785,29 +751,17 @@ resources.ApplyResources(this.jumpwPToolStripMenuItem, "jumpwPToolStripMenuItem"); this.jumpwPToolStripMenuItem.Click += new System.EventHandler(this.jumpwPToolStripMenuItem_Click); // - // createWpCircleToolStripMenuItem + // clearMissionToolStripMenuItem // - this.createWpCircleToolStripMenuItem.Name = "createWpCircleToolStripMenuItem"; - resources.ApplyResources(this.createWpCircleToolStripMenuItem, "createWpCircleToolStripMenuItem"); - this.createWpCircleToolStripMenuItem.Click += new System.EventHandler(this.createWpCircleToolStripMenuItem_Click); + this.clearMissionToolStripMenuItem.Name = "clearMissionToolStripMenuItem"; + resources.ApplyResources(this.clearMissionToolStripMenuItem, "clearMissionToolStripMenuItem"); + this.clearMissionToolStripMenuItem.Click += new System.EventHandler(this.clearMissionToolStripMenuItem_Click); // // toolStripSeparator1 // this.toolStripSeparator1.Name = "toolStripSeparator1"; resources.ApplyResources(this.toolStripSeparator1, "toolStripSeparator1"); // - // ContextMeasure - // - this.ContextMeasure.Name = "ContextMeasure"; - resources.ApplyResources(this.ContextMeasure, "ContextMeasure"); - this.ContextMeasure.Click += new System.EventHandler(this.ContextMeasure_Click); - // - // rotateMapToolStripMenuItem - // - this.rotateMapToolStripMenuItem.Name = "rotateMapToolStripMenuItem"; - resources.ApplyResources(this.rotateMapToolStripMenuItem, "rotateMapToolStripMenuItem"); - this.rotateMapToolStripMenuItem.Click += new System.EventHandler(this.rotateMapToolStripMenuItem_Click); - // // polygonToolStripMenuItem // this.polygonToolStripMenuItem.DropDownItems.AddRange(new System.Windows.Forms.ToolStripItem[] { @@ -881,11 +835,63 @@ resources.ApplyResources(this.saveToFileToolStripMenuItem, "saveToFileToolStripMenuItem"); this.saveToFileToolStripMenuItem.Click += new System.EventHandler(this.saveToFileToolStripMenuItem_Click); // - // clearMissionToolStripMenuItem + // autoWPToolStripMenuItem // - this.clearMissionToolStripMenuItem.Name = "clearMissionToolStripMenuItem"; - resources.ApplyResources(this.clearMissionToolStripMenuItem, "clearMissionToolStripMenuItem"); - this.clearMissionToolStripMenuItem.Click += new System.EventHandler(this.clearMissionToolStripMenuItem_Click); + this.autoWPToolStripMenuItem.DropDownItems.AddRange(new System.Windows.Forms.ToolStripItem[] { + this.createWpCircleToolStripMenuItem, + this.gridToolStripMenuItem}); + this.autoWPToolStripMenuItem.Name = "autoWPToolStripMenuItem"; + resources.ApplyResources(this.autoWPToolStripMenuItem, "autoWPToolStripMenuItem"); + // + // createWpCircleToolStripMenuItem + // + this.createWpCircleToolStripMenuItem.Name = "createWpCircleToolStripMenuItem"; + resources.ApplyResources(this.createWpCircleToolStripMenuItem, "createWpCircleToolStripMenuItem"); + // + // gridToolStripMenuItem + // + this.gridToolStripMenuItem.Name = "gridToolStripMenuItem"; + resources.ApplyResources(this.gridToolStripMenuItem, "gridToolStripMenuItem"); + this.gridToolStripMenuItem.Click += new System.EventHandler(this.gridToolStripMenuItem_Click); + // + // mapToolToolStripMenuItem + // + this.mapToolToolStripMenuItem.DropDownItems.AddRange(new System.Windows.Forms.ToolStripItem[] { + this.ContextMeasure, + this.rotateMapToolStripMenuItem, + this.zoomToToolStripMenuItem, + this.prefetchToolStripMenuItem, + this.kMLOverlayToolStripMenuItem}); + this.mapToolToolStripMenuItem.Name = "mapToolToolStripMenuItem"; + resources.ApplyResources(this.mapToolToolStripMenuItem, "mapToolToolStripMenuItem"); + // + // ContextMeasure + // + this.ContextMeasure.Name = "ContextMeasure"; + resources.ApplyResources(this.ContextMeasure, "ContextMeasure"); + // + // rotateMapToolStripMenuItem + // + this.rotateMapToolStripMenuItem.Name = "rotateMapToolStripMenuItem"; + resources.ApplyResources(this.rotateMapToolStripMenuItem, "rotateMapToolStripMenuItem"); + // + // zoomToToolStripMenuItem + // + this.zoomToToolStripMenuItem.Name = "zoomToToolStripMenuItem"; + resources.ApplyResources(this.zoomToToolStripMenuItem, "zoomToToolStripMenuItem"); + this.zoomToToolStripMenuItem.Click += new System.EventHandler(this.zoomToToolStripMenuItem_Click); + // + // prefetchToolStripMenuItem + // + this.prefetchToolStripMenuItem.Name = "prefetchToolStripMenuItem"; + resources.ApplyResources(this.prefetchToolStripMenuItem, "prefetchToolStripMenuItem"); + this.prefetchToolStripMenuItem.Click += new System.EventHandler(this.prefetchToolStripMenuItem_Click); + // + // kMLOverlayToolStripMenuItem + // + this.kMLOverlayToolStripMenuItem.Name = "kMLOverlayToolStripMenuItem"; + resources.ApplyResources(this.kMLOverlayToolStripMenuItem, "kMLOverlayToolStripMenuItem"); + this.kMLOverlayToolStripMenuItem.Click += new System.EventHandler(this.kMLOverlayToolStripMenuItem_Click); // // trackBar1 // @@ -928,7 +934,6 @@ this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; this.BackColor = System.Drawing.SystemColors.Control; this.Controls.Add(this.panelBASE); - this.MinimumSize = new System.Drawing.Size(1008, 461); this.Name = "FlightPlanner"; this.FormClosing += new System.Windows.Forms.FormClosingEventHandler(this.FlightPlanner_FormClosing); this.Load += new System.EventHandler(this.Planner_Load); @@ -1003,12 +1008,8 @@ private BSE.Windows.Forms.Splitter splitter1; private System.Windows.Forms.Panel panelBASE; private System.Windows.Forms.Label lbl_homedist; - private ArdupilotMega.Controls.MyButton BUT_Prefetch; - private ArdupilotMega.Controls.MyButton BUT_grid; private System.Windows.Forms.ContextMenuStrip contextMenuStrip1; - private System.Windows.Forms.ToolStripMenuItem ContextMeasure; private System.Windows.Forms.ToolTip toolTip1; - private System.Windows.Forms.ToolStripMenuItem rotateMapToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem clearMissionToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem polygonToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem addPolygonPointToolStripMenuItem; @@ -1034,8 +1035,6 @@ private System.Windows.Forms.DataGridViewButtonColumn Delete; private System.Windows.Forms.DataGridViewImageColumn Up; private System.Windows.Forms.DataGridViewImageColumn Down; - private ArdupilotMega.Controls.MyButton BUT_zoomto; - private ArdupilotMega.Controls.MyButton BUT_loadkml; private System.Windows.Forms.Timer timer1; private System.Windows.Forms.ToolStripMenuItem geoFenceToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem GeoFencedownloadToolStripMenuItem; @@ -1045,6 +1044,15 @@ private System.Windows.Forms.ToolStripMenuItem toolStripMenuItem1; private System.Windows.Forms.ToolStripMenuItem GeoFenceuploadToolStripMenuItem; private System.Windows.Forms.ToolStripSeparator toolStripSeparator4; + private System.Windows.Forms.ToolStripMenuItem setROIToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem autoWPToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem createWpCircleToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem gridToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem mapToolToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem ContextMeasure; + private System.Windows.Forms.ToolStripMenuItem rotateMapToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem zoomToToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem prefetchToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem kMLOverlayToolStripMenuItem; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index 5be8d2afbc..038e09af3e 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -40,7 +40,6 @@ namespace ArdupilotMega.GCSViews public static List pointlist = new List(); // used to calc distance static public Object thisLock = new Object(); - private TextBox textBox1; private ComponentResourceManager rm = new ComponentResourceManager(typeof(FlightPlanner)); private Dictionary cmdParamNames = new Dictionary(); @@ -1420,7 +1419,11 @@ namespace ArdupilotMega.GCSViews temp.p3 = (float)(double.Parse(Commands.Rows[a].Cells[Param3.Index].Value.ToString())); temp.p4 = (float)(double.Parse(Commands.Rows[a].Cells[Param4.Index].Value.ToString())); - port.setWP(temp, (ushort)(a + 1), frame, 0); + MAVLink.MAV_MISSION_RESULT ans = port.setWP(temp, (ushort)(a + 1), frame, 0); + if (ans != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED) + { + throw new Exception("Upload WPs Failed " + Commands.Rows[a].Cells[Command.Index].Value.ToString() + " " + Enum.Parse(typeof(MAVLink.MAV_MISSION_RESULT), ans.ToString())); + } } port.setWPACK(); @@ -2434,42 +2437,7 @@ namespace ArdupilotMega.GCSViews private void BUT_Prefetch_Click(object sender, EventArgs e) { - RectLatLng area = MainMap.SelectedArea; - if (area.IsEmpty) - { - DialogResult res = CustomMessageBox.Show("No ripp area defined, ripp displayed on screen?", "Rip", MessageBoxButtons.YesNo); - if (res == DialogResult.Yes) - { - area = MainMap.CurrentViewArea; - } - } - - if (!area.IsEmpty) - { - DialogResult res = CustomMessageBox.Show("Ready ripp at Zoom = " + (int)MainMap.Zoom + " ?", "GMap.NET", MessageBoxButtons.YesNo); - - for (int i = 1; i <= MainMap.MaxZoom; i++) - { - if (res == DialogResult.Yes) - { - TilePrefetcher obj = new TilePrefetcher(); - obj.ShowCompleteMessage = false; - obj.Start(area, MainMap.Projection, i, MainMap.MapType, 100); - } - else if (res == DialogResult.No) - { - continue; - } - else if (res == DialogResult.Cancel) - { - break; - } - } - } - else - { - CustomMessageBox.Show("Select map area holding ALT", "GMap.NET", MessageBoxButtons.OK, MessageBoxIcon.Exclamation); - } + } /// @@ -2527,232 +2495,7 @@ namespace ArdupilotMega.GCSViews private void BUT_grid_Click(object sender, EventArgs e) { - polygongridmode = false; - - if (drawnpolygon == null || drawnpolygon.Points.Count == 0) - { - CustomMessageBox.Show("Right click the map to draw a polygon", "Area", MessageBoxButtons.OK, MessageBoxIcon.Exclamation); - return; - } - - // ensure points/latlong are current - MainMap.Zoom = (int)MainMap.Zoom; - - MainMap.Refresh(); - - GMapPolygon area = drawnpolygon; - area.Points.Add(area.Points[0]); // make a full loop - RectLatLng arearect = getPolyMinMax(area); - if (area.Distance > 0) - { - - PointLatLng topright = new PointLatLng(arearect.LocationTopLeft.Lat, arearect.LocationRightBottom.Lng); - PointLatLng bottomleft = new PointLatLng(arearect.LocationRightBottom.Lat, arearect.LocationTopLeft.Lng); - - double diagdist = MainMap.Manager.GetDistance(arearect.LocationTopLeft, arearect.LocationRightBottom) * 1000; - double heightdist = MainMap.Manager.GetDistance(arearect.LocationTopLeft, bottomleft) * 1000; - double widthdist = MainMap.Manager.GetDistance(arearect.LocationTopLeft, topright) * 1000; - - - - string alt = (100 * MainV2.cs.multiplierdist).ToString("0"); - Common.InputBox("Altitude", "Relative Altitude", ref alt); - - - string distance = (50 * MainV2.cs.multiplierdist).ToString("0"); - Common.InputBox("Distance", "Distance between lines", ref distance); - - //string overshoot = (30 * MainV2.cs.multiplierdist).ToString("0"); - //Common.InputBox("Overshoot", "Enter of line overshoot amount", ref overshoot); - - string angle = (90).ToString("0"); - Common.InputBox("Angle", "Enter the line direction (0-180)", ref angle); - - double tryme = 0; - - if (!double.TryParse(angle, out tryme)) - { - CustomMessageBox.Show("Invalid Angle"); - return; - } - if (!double.TryParse(alt, out tryme)) - { - CustomMessageBox.Show("Invalid Alt"); - return; - } - if (!double.TryParse(distance, out tryme)) - { - CustomMessageBox.Show("Invalid Distance"); - return; - } - -#if DEBUG - //Commands.Rows.Clear(); -#endif - // get x y components - double x1 = Math.Cos((double.Parse(angle)) * deg2rad); // needs to mod for long scale - double y1 = Math.Sin((double.Parse(angle)) * deg2rad); - - // get x y step amount in lat lng from m - double latdiff = arearect.HeightLat / ((heightdist / (double.Parse(distance) * (y1) / MainV2.cs.multiplierdist))); - double lngdiff = arearect.WidthLng / ((widthdist / (double.Parse(distance) * (x1) / MainV2.cs.multiplierdist))); - - double latlngdiff = Math.Sqrt(latdiff * latdiff + lngdiff * lngdiff); - - double fulllatdiff = arearect.HeightLat * x1 * 2; - double fulllngdiff = arearect.WidthLng * y1 * 2; - - // lat - up down - // lng - left right - - int overshootdist = 0;// (int)(double.Parse(overshoot) / MainV2.cs.multiplierdist); - - int altitude = (int)(double.Parse(alt) / MainV2.cs.multiplierdist); - - double overshootdistlng = arearect.WidthLng / widthdist * overshootdist; - - bool dir = false; - - int count = 0; - - double x = bottomleft.Lat - Math.Abs(fulllatdiff); - double y = bottomleft.Lng - Math.Abs(fulllngdiff); - - log.InfoFormat("{0} < {1} {2} < {3}", x, (topright.Lat), y, (topright.Lng)); - - while (x < (topright.Lat + Math.Abs(fulllatdiff)) && y < (topright.Lng + Math.Abs(fulllngdiff))) - { - if (double.Parse(angle) < 45) - { - x = bottomleft.Lat; - y += latlngdiff; - } - else if (double.Parse(angle) > 135) - { - x = arearect.LocationTopLeft.Lat; //arearect.LocationTopLeft.Lat; - y += latlngdiff; - } - else if (double.Parse(angle) > 90) - { - y = bottomleft.Lng; //arearect.LocationTopLeft.Lat; - x += latlngdiff; - } - else - { - y = bottomleft.Lng; - x += latlngdiff; - } - - //callMe(x , y, 0); - //callMe(x + (fulllatdiff), y + (fulllngdiff), 0); - - //continue; - - PointLatLng closestlatlong = PointLatLng.Zero; - PointLatLng farestlatlong = PointLatLng.Zero; - - double noc = double.MaxValue; - double nof = double.MinValue; - - if (dir) - { - double ax = x; - double ay = y; - - double bx = x + fulllatdiff; - double by = y + fulllngdiff; - int a = -1; - PointLatLng newlatlong = PointLatLng.Zero; - foreach (PointLatLng pnt in area.Points) - { - a++; - if (a == 0) - { - continue; - } - newlatlong = FindLineIntersection(area.Points[a - 1], area.Points[a], new PointLatLng(ax, ay), new PointLatLng(bx, by)); - if (!newlatlong.IsZero) - { - if (noc > MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong)) - { - closestlatlong.Lat = newlatlong.Lat; - closestlatlong.Lng = newlatlong.Lng; - noc = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong); - } - if (nof < MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong)) - { - farestlatlong.Lat = newlatlong.Lat; - farestlatlong.Lng = newlatlong.Lng; - nof = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong); - } - } - } - - if (!farestlatlong.IsZero) - callMe(farestlatlong.Lat, farestlatlong.Lng, altitude); - if (!closestlatlong.IsZero) - callMe(closestlatlong.Lat, closestlatlong.Lng - overshootdistlng, altitude); - - //callMe(x, topright.Lng, altitude); - //callMe(x, bottomleft.Lng - overshootdistlng, altitude); - } - else - { - double ax = x; - double ay = y; - - double bx = x + fulllatdiff; - double by = y + fulllngdiff; - int a = -1; - PointLatLng newlatlong = PointLatLng.Zero; - foreach (PointLatLng pnt in area.Points) - { - a++; - if (a == 0) - { - continue; - } - newlatlong = FindLineIntersection(area.Points[a - 1], area.Points[a], new PointLatLng(ax, ay), new PointLatLng(bx, by)); - if (!newlatlong.IsZero) - { - if (noc > MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong)) - { - closestlatlong.Lat = newlatlong.Lat; - closestlatlong.Lng = newlatlong.Lng; - noc = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong); - } - if (nof < MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong)) - { - farestlatlong.Lat = newlatlong.Lat; - farestlatlong.Lng = newlatlong.Lng; - nof = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong); - } - } - } - if (!closestlatlong.IsZero) - callMe(closestlatlong.Lat, closestlatlong.Lng, altitude); - if (!farestlatlong.IsZero) - callMe(farestlatlong.Lat, farestlatlong.Lng + overshootdistlng, altitude); - //callMe(x, bottomleft.Lng, altitude); - //callMe(x, topright.Lng + overshootdistlng, altitude); - } - - dir = !dir; - - count++; - - if (Commands.RowCount > 150) - { - CustomMessageBox.Show("Stopping at 150 WP's"); - break; - } - } - - //drawnpolygon.Points.Clear(); - //drawnpolygons.Markers.Clear(); - MainMap.Refresh(); - - } + } private void label4_LinkClicked(object sender, LinkLabelLinkClickedEventArgs e) @@ -3027,64 +2770,12 @@ namespace ArdupilotMega.GCSViews private void BUT_zoomto_Click(object sender, EventArgs e) { - string place = "Perth Airport, Australia"; - if (DialogResult.OK == Common.InputBox("Location", "Enter your location", ref place)) - { - - GeoCoderStatusCode status = MainMap.SetCurrentPositionByKeywords(place); - if (status != GeoCoderStatusCode.G_GEO_SUCCESS) - { - CustomMessageBox.Show("Google Maps Geocoder can't find: '" + place + "', reason: " + status.ToString(), "GMap.NET", MessageBoxButtons.OK, MessageBoxIcon.Exclamation); - } - else - { - MainMap.Zoom = 15; - } - } + } private void BUT_loadkml_Click(object sender, EventArgs e) { - OpenFileDialog fd = new OpenFileDialog(); - fd.Filter = "Google Earth KML (*.kml)|*.kml"; - fd.DefaultExt = ".kml"; - DialogResult result = fd.ShowDialog(); - string file = fd.FileName; - if (file != "") - { - try - { - 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("", ""); - - var parser = new SharpKml.Base.Parser(); - - parser.ElementAdded += parser_ElementAdded; - parser.ParseString(kml, true); - - if (DialogResult.Yes == CustomMessageBox.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) { CustomMessageBox.Show("Bad KML File :" + ex.ToString()); } - } } @@ -3502,5 +3193,352 @@ namespace ArdupilotMega.GCSViews { timer1.Stop(); } + + private void setROIToolStripMenuItem_Click(object sender, EventArgs e) + { + selectedrow = Commands.Rows.Add(); + + Commands.Rows[selectedrow].Cells[Command.Index].Value = MAVLink.MAV_CMD.ROI.ToString(); + + //Commands.Rows[selectedrow].Cells[Param1.Index].Value = time; + + ChangeColumnHeader(MAVLink.MAV_CMD.ROI.ToString()); + + setfromGE(end.Lat, end.Lng, (int)float.Parse(TXT_DefaultAlt.Text)); + + writeKML(); + } + + private void gridToolStripMenuItem_Click(object sender, EventArgs e) + { + polygongridmode = false; + + if (drawnpolygon == null || drawnpolygon.Points.Count == 0) + { + CustomMessageBox.Show("Right click the map to draw a polygon", "Area", MessageBoxButtons.OK, MessageBoxIcon.Exclamation); + return; + } + + // ensure points/latlong are current + MainMap.Zoom = (int)MainMap.Zoom; + + MainMap.Refresh(); + + GMapPolygon area = drawnpolygon; + area.Points.Add(area.Points[0]); // make a full loop + RectLatLng arearect = getPolyMinMax(area); + if (area.Distance > 0) + { + + PointLatLng topright = new PointLatLng(arearect.LocationTopLeft.Lat, arearect.LocationRightBottom.Lng); + PointLatLng bottomleft = new PointLatLng(arearect.LocationRightBottom.Lat, arearect.LocationTopLeft.Lng); + + double diagdist = MainMap.Manager.GetDistance(arearect.LocationTopLeft, arearect.LocationRightBottom) * 1000; + double heightdist = MainMap.Manager.GetDistance(arearect.LocationTopLeft, bottomleft) * 1000; + double widthdist = MainMap.Manager.GetDistance(arearect.LocationTopLeft, topright) * 1000; + + + + string alt = (100 * MainV2.cs.multiplierdist).ToString("0"); + Common.InputBox("Altitude", "Relative Altitude", ref alt); + + + string distance = (50 * MainV2.cs.multiplierdist).ToString("0"); + Common.InputBox("Distance", "Distance between lines", ref distance); + + //string overshoot = (30 * MainV2.cs.multiplierdist).ToString("0"); + //Common.InputBox("Overshoot", "Enter of line overshoot amount", ref overshoot); + + string angle = (90).ToString("0"); + Common.InputBox("Angle", "Enter the line direction (0-180)", ref angle); + + double tryme = 0; + + if (!double.TryParse(angle, out tryme)) + { + CustomMessageBox.Show("Invalid Angle"); + return; + } + if (!double.TryParse(alt, out tryme)) + { + CustomMessageBox.Show("Invalid Alt"); + return; + } + if (!double.TryParse(distance, out tryme)) + { + CustomMessageBox.Show("Invalid Distance"); + return; + } + +#if DEBUG + //Commands.Rows.Clear(); +#endif + // get x y components + double x1 = Math.Cos((double.Parse(angle)) * deg2rad); // needs to mod for long scale + double y1 = Math.Sin((double.Parse(angle)) * deg2rad); + + // get x y step amount in lat lng from m + double latdiff = arearect.HeightLat / ((heightdist / (double.Parse(distance) * (y1) / MainV2.cs.multiplierdist))); + double lngdiff = arearect.WidthLng / ((widthdist / (double.Parse(distance) * (x1) / MainV2.cs.multiplierdist))); + + double latlngdiff = Math.Sqrt(latdiff * latdiff + lngdiff * lngdiff); + + double fulllatdiff = arearect.HeightLat * x1 * 2; + double fulllngdiff = arearect.WidthLng * y1 * 2; + + // lat - up down + // lng - left right + + int overshootdist = 0;// (int)(double.Parse(overshoot) / MainV2.cs.multiplierdist); + + int altitude = (int)(double.Parse(alt) / MainV2.cs.multiplierdist); + + double overshootdistlng = arearect.WidthLng / widthdist * overshootdist; + + bool dir = false; + + int count = 0; + + double x = bottomleft.Lat - Math.Abs(fulllatdiff); + double y = bottomleft.Lng - Math.Abs(fulllngdiff); + + log.InfoFormat("{0} < {1} {2} < {3}", x, (topright.Lat), y, (topright.Lng)); + + while (x < (topright.Lat + Math.Abs(fulllatdiff)) && y < (topright.Lng + Math.Abs(fulllngdiff))) + { + if (double.Parse(angle) < 45) + { + x = bottomleft.Lat; + y += latlngdiff; + } + else if (double.Parse(angle) > 135) + { + x = arearect.LocationTopLeft.Lat; //arearect.LocationTopLeft.Lat; + y += latlngdiff; + } + else if (double.Parse(angle) > 90) + { + y = bottomleft.Lng; //arearect.LocationTopLeft.Lat; + x += latlngdiff; + } + else + { + y = bottomleft.Lng; + x += latlngdiff; + } + + //callMe(x , y, 0); + //callMe(x + (fulllatdiff), y + (fulllngdiff), 0); + + //continue; + + PointLatLng closestlatlong = PointLatLng.Zero; + PointLatLng farestlatlong = PointLatLng.Zero; + + double noc = double.MaxValue; + double nof = double.MinValue; + + if (dir) + { + double ax = x; + double ay = y; + + double bx = x + fulllatdiff; + double by = y + fulllngdiff; + int a = -1; + PointLatLng newlatlong = PointLatLng.Zero; + foreach (PointLatLng pnt in area.Points) + { + a++; + if (a == 0) + { + continue; + } + newlatlong = FindLineIntersection(area.Points[a - 1], area.Points[a], new PointLatLng(ax, ay), new PointLatLng(bx, by)); + if (!newlatlong.IsZero) + { + if (noc > MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong)) + { + closestlatlong.Lat = newlatlong.Lat; + closestlatlong.Lng = newlatlong.Lng; + noc = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong); + } + if (nof < MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong)) + { + farestlatlong.Lat = newlatlong.Lat; + farestlatlong.Lng = newlatlong.Lng; + nof = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong); + } + } + } + + if (!farestlatlong.IsZero) + callMe(farestlatlong.Lat, farestlatlong.Lng, altitude); + if (!closestlatlong.IsZero) + callMe(closestlatlong.Lat, closestlatlong.Lng - overshootdistlng, altitude); + + //callMe(x, topright.Lng, altitude); + //callMe(x, bottomleft.Lng - overshootdistlng, altitude); + } + else + { + double ax = x; + double ay = y; + + double bx = x + fulllatdiff; + double by = y + fulllngdiff; + int a = -1; + PointLatLng newlatlong = PointLatLng.Zero; + foreach (PointLatLng pnt in area.Points) + { + a++; + if (a == 0) + { + continue; + } + newlatlong = FindLineIntersection(area.Points[a - 1], area.Points[a], new PointLatLng(ax, ay), new PointLatLng(bx, by)); + if (!newlatlong.IsZero) + { + if (noc > MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong)) + { + closestlatlong.Lat = newlatlong.Lat; + closestlatlong.Lng = newlatlong.Lng; + noc = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong); + } + if (nof < MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong)) + { + farestlatlong.Lat = newlatlong.Lat; + farestlatlong.Lng = newlatlong.Lng; + nof = MainMap.Manager.GetDistance(new PointLatLng(ax, ay), newlatlong); + } + } + } + if (!closestlatlong.IsZero) + callMe(closestlatlong.Lat, closestlatlong.Lng, altitude); + if (!farestlatlong.IsZero) + callMe(farestlatlong.Lat, farestlatlong.Lng + overshootdistlng, altitude); + //callMe(x, bottomleft.Lng, altitude); + //callMe(x, topright.Lng + overshootdistlng, altitude); + } + + dir = !dir; + + count++; + + if (Commands.RowCount > 150) + { + CustomMessageBox.Show("Stopping at 150 WP's"); + break; + } + } + + //drawnpolygon.Points.Clear(); + //drawnpolygons.Markers.Clear(); + MainMap.Refresh(); + + } + } + + private void zoomToToolStripMenuItem_Click(object sender, EventArgs e) + { + string place = "Perth Airport, Australia"; + if (DialogResult.OK == Common.InputBox("Location", "Enter your location", ref place)) + { + + GeoCoderStatusCode status = MainMap.SetCurrentPositionByKeywords(place); + if (status != GeoCoderStatusCode.G_GEO_SUCCESS) + { + CustomMessageBox.Show("Google Maps Geocoder can't find: '" + place + "', reason: " + status.ToString(), "GMap.NET", MessageBoxButtons.OK, MessageBoxIcon.Exclamation); + } + else + { + MainMap.Zoom = 15; + } + } + } + + private void prefetchToolStripMenuItem_Click(object sender, EventArgs e) + { + RectLatLng area = MainMap.SelectedArea; + if (area.IsEmpty) + { + DialogResult res = CustomMessageBox.Show("No ripp area defined, ripp displayed on screen?", "Rip", MessageBoxButtons.YesNo); + if (res == DialogResult.Yes) + { + area = MainMap.CurrentViewArea; + } + } + + if (!area.IsEmpty) + { + DialogResult res = CustomMessageBox.Show("Ready ripp at Zoom = " + (int)MainMap.Zoom + " ?", "GMap.NET", MessageBoxButtons.YesNo); + + for (int i = 1; i <= MainMap.MaxZoom; i++) + { + if (res == DialogResult.Yes) + { + TilePrefetcher obj = new TilePrefetcher(); + obj.ShowCompleteMessage = false; + obj.Start(area, MainMap.Projection, i, MainMap.MapType, 100); + } + else if (res == DialogResult.No) + { + continue; + } + else if (res == DialogResult.Cancel) + { + break; + } + } + } + else + { + CustomMessageBox.Show("Select map area holding ALT", "GMap.NET", MessageBoxButtons.OK, MessageBoxIcon.Exclamation); + } + } + + private void kMLOverlayToolStripMenuItem_Click(object sender, EventArgs e) + { + + OpenFileDialog fd = new OpenFileDialog(); + fd.Filter = "Google Earth KML (*.kml)|*.kml"; + fd.DefaultExt = ".kml"; + DialogResult result = fd.ShowDialog(); + string file = fd.FileName; + if (file != "") + { + try + { + 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("", ""); + + var parser = new SharpKml.Base.Parser(); + + parser.ElementAdded += parser_ElementAdded; + parser.ParseString(kml, true); + + if (DialogResult.Yes == CustomMessageBox.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) { CustomMessageBox.Show("Bad KML File :" + ex.ToString()); } + } + } } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.resx b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.resx index 86587af632..aa13161700 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.resx @@ -148,7 +148,7 @@ panelWaypoints - 5 + 1 True @@ -178,7 +178,7 @@ panelWaypoints - 10 + 6 Top, Bottom, Left, Right @@ -322,10 +322,10 @@ 10, 55 - 75 + 35 - 862, 89 + 601, 89 6 @@ -340,7 +340,7 @@ panelWaypoints - 12 + 8 True @@ -370,7 +370,7 @@ panelWaypoints - 14 + 10 23, 36 @@ -394,7 +394,7 @@ panelWaypoints - 15 + 11 154, 36 @@ -418,7 +418,7 @@ panelWaypoints - 13 + 9 True @@ -448,7 +448,7 @@ panelWaypoints - 6 + 2 True @@ -478,7 +478,7 @@ panelWaypoints - 11 + 7 89, 36 @@ -502,7 +502,7 @@ panelWaypoints - 9 + 5 True @@ -532,7 +532,7 @@ panelWaypoints - 8 + 4 Bottom, Right @@ -556,7 +556,7 @@ BUT_write - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.25609, Culture=neutral, PublicKeyToken=null panel5 @@ -583,7 +583,7 @@ BUT_read - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.25609, Culture=neutral, PublicKeyToken=null panel5 @@ -610,7 +610,7 @@ SaveFile - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.25609, Culture=neutral, PublicKeyToken=null panel5 @@ -637,7 +637,7 @@ BUT_loadwpfile - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.25609, Culture=neutral, PublicKeyToken=null panel5 @@ -664,7 +664,7 @@ panelAction - 1 + 0 Bottom, Right @@ -871,7 +871,7 @@ panelAction - 2 + 1 Up @@ -1114,7 +1114,7 @@ panelAction - 3 + 2 Bottom, Right @@ -1144,7 +1144,7 @@ panelAction - 4 + 3 Bottom, Right @@ -1177,40 +1177,7 @@ panelAction - 5 - - - Bottom, Right - - - 7, 25 - - - True - - - 117, 103 - - - 48 - - - 1. Connect -2. Read WP's if you need to. -3. Ensure your Home location and ALT is set -4. Click on the map to start adding WP's - - - textBox1 - - - System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - panelAction - - - 0 + 4 Bottom @@ -1222,7 +1189,7 @@ 0, 310 - 878, 3 + 617, 3 50 @@ -1239,71 +1206,14 @@ 0 - - NoControl - - - 800, 25 - - - 58, 23 - - - 54 - - - KML overlay - - - Get Camera settings for overlap - - - BUT_loadkml - - - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null - - - panelWaypoints - - - 0 - - - NoControl - - - 732, 26 - - - 62, 23 - - - 53 - - - Zoom To - - - Get Camera settings for overlap - - - BUT_zoomto - - - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null - - - panelWaypoints - - - 1 + + True NoControl - 678, 26 + 562, 26 48, 23 @@ -1321,73 +1231,13 @@ BUT_Camera - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.25609, Culture=neutral, PublicKeyToken=null panelWaypoints - 2 - - - NoControl - - - 624, 26 - - - 48, 23 - - - 51 - - - Grid - - - Draws a grid over a pre defined area with a given spacing - - - BUT_grid - - - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null - - - panelWaypoints - - - 3 - - - NoControl - - - 562, 26 - - - 56, 23 - - - 50 - - - Prefetch - - - Pre Caches a part of the map based on a box you draw - - - BUT_Prefetch - - - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null - - - panelWaypoints - - - 4 + 0 NoControl @@ -1411,13 +1261,13 @@ button1 - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.25609, Culture=neutral, PublicKeyToken=null panelWaypoints - 7 + 3 NoControl @@ -1441,13 +1291,13 @@ BUT_Add - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4606.25609, Culture=neutral, PublicKeyToken=null panelWaypoints - 16 + 12 Bottom @@ -1456,7 +1306,7 @@ 0, 313 - 878, 148 + 617, 148 49 @@ -1480,7 +1330,7 @@ Right - 878, 0 + 617, 0 130, 461 @@ -1600,11 +1450,17 @@ 17, 17 - 167, 22 + 148, 22 Delete WP + + 148, 22 + + + Set ROI + 113, 22 @@ -1624,7 +1480,7 @@ Circles - 167, 22 + 148, 22 Loiter @@ -1642,31 +1498,19 @@ WP # - 167, 22 + 148, 22 Jump - - 167, 22 + + 148, 22 - - Create Wp Circle + + Clear Mission - 164, 6 - - - 167, 22 - - - Measure Distance - - - 167, 22 - - - Rotate Map + 145, 6 174, 22 @@ -1681,7 +1525,7 @@ Clear Polygon - 167, 22 + 148, 22 Draw Polygon @@ -1726,19 +1570,67 @@ Save to File - 167, 22 + 148, 22 Geo-Fence - + + 162, 22 + + + Create Wp Circle + + + 162, 22 + + + Grid + + + 148, 22 + + + Auto WP + + 167, 22 - - Clear Mission + + Measure Distance + + + 167, 22 + + + Rotate Map + + + 167, 22 + + + Zoom To + + + 167, 22 + + + Prefetch + + + 167, 22 + + + KML Overlay + + + 148, 22 + + + Map Tool - 168, 230 + 149, 208 contextMenuStrip1 @@ -1750,7 +1642,7 @@ 0, 0 - 838, 306 + 577, 306 @@ -1904,7 +1796,7 @@ MainMap - ArdupilotMega.Controls.myGMAP, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.myGMAP, ArdupilotMegaPlanner10, Version=1.1.4606.25609, Culture=neutral, PublicKeyToken=null panelMap @@ -1919,7 +1811,7 @@ NoControl - 847, 21 + 586, 21 Vertical @@ -1934,7 +1826,7 @@ trackBar1 - ArdupilotMega.Controls.MyTrackBar, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null + ArdupilotMega.Controls.MyTrackBar, ArdupilotMegaPlanner10, Version=1.1.4606.25609, Culture=neutral, PublicKeyToken=null panelMap @@ -1952,7 +1844,7 @@ NoControl - 844, 5 + 583, 5 34, 13 @@ -1982,7 +1874,7 @@ 0, 0 - 878, 313 + 617, 313 51 @@ -2009,7 +1901,7 @@ 0, 0 - 1008, 461 + 747, 461 52 @@ -2039,7 +1931,7 @@ 0, 0, 0, 0 - 1008, 461 + 747, 461 Command @@ -2125,6 +2017,12 @@ System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + setROIToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + loiterToolStripMenuItem @@ -2167,10 +2065,10 @@ System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - createWpCircleToolStripMenuItem + + clearMissionToolStripMenuItem - + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 @@ -2179,18 +2077,6 @@ System.Windows.Forms.ToolStripSeparator, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - ContextMeasure - - - System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - rotateMapToolStripMenuItem - - - System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - polygonToolStripMenuItem @@ -2257,10 +2143,58 @@ System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - clearMissionToolStripMenuItem + + autoWPToolStripMenuItem - + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + createWpCircleToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + gridToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + mapToolToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + ContextMeasure + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + rotateMapToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + zoomToToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + prefetchToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + kMLOverlayToolStripMenuItem + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 @@ -2279,6 +2213,6 @@ FlightPlanner - System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner10, Version=1.1.4582.39185, Culture=neutral, PublicKeyToken=null + System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner10, Version=1.1.4606.25609, Culture=neutral, PublicKeyToken=null \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Help.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Help.Designer.cs index 9151f64fdb..235481a054 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Help.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Help.Designer.cs @@ -38,6 +38,7 @@ // resources.ApplyResources(this.richTextBox1, "richTextBox1"); this.richTextBox1.Cursor = System.Windows.Forms.Cursors.Default; + this.richTextBox1.DetectUrls = false; this.richTextBox1.Name = "richTextBox1"; // // CHK_showconsole diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Help.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Help.resx index f5e2d6cd42..12af2e1179 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Help.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Help.resx @@ -196,7 +196,7 @@ BUT_updatecheck - ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.Controls.MyButton, ArdupilotMegaPlanner10, Version=1.1.4605.13518, Culture=neutral, PublicKeyToken=null $this @@ -204,9 +204,9 @@ 1 - + True - + 6, 13 @@ -220,7 +220,7 @@ Help - System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner10, Version=1.1.4605.13518, Culture=neutral, PublicKeyToken=null ..\Resources\Welcome_to_Michael_Oborne.rtf;System.String, mscorlib, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089;Windows-1252 diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs index 7048d226a5..7754e1bb72 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs @@ -16,6 +16,8 @@ using System.Reflection; using ArdupilotMega.Controls; using System.Drawing.Drawing2D; +using ArdupilotMega.HIL; + // Written by Michael Oborne namespace ArdupilotMega.GCSViews { @@ -697,6 +699,10 @@ namespace ArdupilotMega.GCSViews XplanesSEND = new UdpClient(simIP, simPort); OutputLog.AppendText("Sending to port UDP " + simPort + " (planner->sim)\n"); + + setupXplane(); + + OutputLog.AppendText("Sent xplane settings\n"); } private void SetupUDPMavLink() @@ -778,19 +784,29 @@ namespace ArdupilotMega.GCSViews sitldata.speedN = DATA[21][3];// (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad)); sitldata.speedE = -DATA[21][5];// (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad)); - // YLScsDrawing.Drawing3d.Vector3d accel3D = HIL.QuadCopter.RPY_to_XYZ(DATA[18][1], DATA[18][0], 0, -9.8); //DATA[18][2] + Matrix3 dcm = new Matrix3(); + dcm.from_euler(sitldata.rollDeg * deg2rad, sitldata.pitchDeg * deg2rad, sitldata.yawDeg * deg2rad); - // float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704) * 1.943844) / (float)(11.26 * Math.Tan(sitldata.rollDeg * deg2rad))) * ft2m; + // rad = tas^2 / (tan(angle) * G) + float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / (float)(9.8f * Math.Tan(sitldata.rollDeg * deg2rad))); - // float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad; + float gload = (float)(1 / Math.Cos(sitldata.rollDeg * deg2rad)); // calculated Gs - // YLScsDrawing.Drawing3d.Vector3d cent3D = HIL.QuadCopter.RPY_to_XYZ(DATA[18][1] - 90, 0, 0, centripaccel); + // a = v^2/r + float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad; - // accel3D -= cent3D; + Vector3 accel_body = dcm.transposed() * (new Vector3(0, 0, -9.8)); - // sitldata.xAccel = accel3D.X; - // sitldata.yAccel = accel3D.Y; - // sitldata.zAccel = accel3D.Z; + Vector3 centrip_accel = new Vector3(0, centripaccel * Math.Cos(sitldata.rollDeg * deg2rad), centripaccel * Math.Sin(sitldata.rollDeg * deg2rad)); + + accel_body -= centrip_accel; + + sitldata.xAccel = DATA[4][5] * 9.8; + sitldata.yAccel = DATA[4][6] * 9.8; + sitldata.zAccel = (0 - DATA[4][4]) * 9.8; + + Console.WriteLine(accel_body.ToString()); + Console.WriteLine(" {0} {1} {2}",sitldata.xAccel, sitldata.yAccel, sitldata.zAccel); } else if (receviedbytes == 0x64) // FG binary udp @@ -908,6 +924,9 @@ namespace ArdupilotMega.GCSViews sitldata.speedE = fdm.v_north * ft2m; sitldata.airspeed = fdm.vcas * 0.5144444f;// knots to m/s + + if (RAD_JSBSim.Checked) + sitldata.airspeed = fdm.vcas * 0.3048f;// fps to m/s } else @@ -1425,6 +1444,72 @@ namespace ArdupilotMega.GCSViews } } + void setupXplane() + { + if (RAD_softXplanes.Checked) + { + + + // sending only 1 packet instead of many. + + byte[] Xplane = new byte[5 + 4 * 8]; + + Xplane[0] = (byte)'D'; + Xplane[1] = (byte)'S'; + Xplane[2] = (byte)'E'; + Xplane[3] = (byte)'L'; + Xplane[4] = 0; + + if (CHK_xplane10.Checked) + { + int pos = 5; + Xplane[pos] = 0x3; + pos += 4; + Xplane[pos] = 0x4; + pos += 4; + Xplane[pos] = 0x6; + pos += 4; + Xplane[pos] = 0x10; + pos += 4; + Xplane[pos] = 0x11; + pos += 4; + Xplane[pos] = 0x12; + pos += 4; + Xplane[pos] = 0x14; + pos += 4; + Xplane[pos] = 0x15; + pos += 4; + } + else + { + int pos = 5; + Xplane[pos] = 0x3; + pos += 4; + Xplane[pos] = 0x4; + pos += 4; + Xplane[pos] = 0x6; + pos += 4; + Xplane[pos] = 0x11; + pos += 4; + Xplane[pos] = 0x12; + pos += 4; + Xplane[pos] = 0x13; + pos += 4; + Xplane[pos] = 0x14; + pos += 4; + Xplane[pos] = 0x15; + pos += 4; + } + + try + { + XplanesSEND.Send(Xplane, Xplane.Length); + + } + catch (Exception e) { log.Info("Xplanes udp send error " + e.Message); } + } + } + byte[] StructureToByteArray(object obj) { diff --git a/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs b/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs index 41fa560efa..b9058d32ed 100644 --- a/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs +++ b/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs @@ -107,7 +107,6 @@ namespace ArdupilotMega.HIL private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); QuadCopter self; - int framecount = 0; DateTime seconds = DateTime.Now; double[] motor_speed = null; diff --git a/Tools/ArdupilotMegaPlanner/MainV2.Designer.cs b/Tools/ArdupilotMegaPlanner/MainV2.Designer.cs index ed83ecc2cd..e30b41ed6e 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.Designer.cs @@ -78,11 +78,13 @@ this.MainMenu.Size = new System.Drawing.Size(1008, 76); this.MainMenu.TabIndex = 5; this.MainMenu.Text = "menuStrip1"; + this.MainMenu.MouseLeave += new System.EventHandler(this.MainMenu_MouseLeave); // // MenuFlightData // this.MenuFlightData.AutoSize = false; this.MenuFlightData.BackgroundImage = global::ArdupilotMega.Properties.Resources.data; + this.MenuFlightData.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom; this.MenuFlightData.DisplayStyle = System.Windows.Forms.ToolStripItemDisplayStyle.Image; this.MenuFlightData.ImageTransparentColor = System.Drawing.Color.Magenta; this.MenuFlightData.Margin = new System.Windows.Forms.Padding(0); @@ -95,6 +97,7 @@ // this.MenuFlightPlanner.AutoSize = false; this.MenuFlightPlanner.BackgroundImage = global::ArdupilotMega.Properties.Resources.planner; + this.MenuFlightPlanner.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom; this.MenuFlightPlanner.DisplayStyle = System.Windows.Forms.ToolStripItemDisplayStyle.Image; this.MenuFlightPlanner.ImageTransparentColor = System.Drawing.Color.Magenta; this.MenuFlightPlanner.Margin = new System.Windows.Forms.Padding(0); @@ -109,6 +112,7 @@ // this.MenuConfiguration.AutoSize = false; this.MenuConfiguration.BackgroundImage = global::ArdupilotMega.Properties.Resources.configuration; + this.MenuConfiguration.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom; this.MenuConfiguration.DisplayStyle = System.Windows.Forms.ToolStripItemDisplayStyle.Image; this.MenuConfiguration.ImageTransparentColor = System.Drawing.Color.Magenta; this.MenuConfiguration.Margin = new System.Windows.Forms.Padding(0); @@ -123,6 +127,7 @@ // this.MenuSimulation.AutoSize = false; this.MenuSimulation.BackgroundImage = global::ArdupilotMega.Properties.Resources.simulation; + this.MenuSimulation.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom; this.MenuSimulation.DisplayStyle = System.Windows.Forms.ToolStripItemDisplayStyle.Image; this.MenuSimulation.ImageTransparentColor = System.Drawing.Color.Magenta; this.MenuSimulation.Margin = new System.Windows.Forms.Padding(0); @@ -137,6 +142,7 @@ // this.MenuFirmware.AutoSize = false; this.MenuFirmware.BackgroundImage = global::ArdupilotMega.Properties.Resources.firmware; + this.MenuFirmware.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom; this.MenuFirmware.DisplayStyle = System.Windows.Forms.ToolStripItemDisplayStyle.Image; this.MenuFirmware.ImageTransparentColor = System.Drawing.Color.Magenta; this.MenuFirmware.Margin = new System.Windows.Forms.Padding(0); @@ -151,6 +157,7 @@ // this.MenuTerminal.AutoSize = false; this.MenuTerminal.BackgroundImage = global::ArdupilotMega.Properties.Resources.terminal; + this.MenuTerminal.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom; this.MenuTerminal.DisplayStyle = System.Windows.Forms.ToolStripItemDisplayStyle.Image; this.MenuTerminal.ImageTransparentColor = System.Drawing.Color.Magenta; this.MenuTerminal.Margin = new System.Windows.Forms.Padding(0); @@ -165,6 +172,7 @@ // this.MenuHelp.AutoSize = false; this.MenuHelp.BackgroundImage = global::ArdupilotMega.Properties.Resources.help; + this.MenuHelp.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom; this.MenuHelp.DisplayStyle = System.Windows.Forms.ToolStripItemDisplayStyle.Image; this.MenuHelp.ImageTransparentColor = System.Drawing.Color.Magenta; this.MenuHelp.Margin = new System.Windows.Forms.Padding(0); @@ -180,6 +188,7 @@ this.MenuConnect.Alignment = System.Windows.Forms.ToolStripItemAlignment.Right; this.MenuConnect.AutoSize = false; this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.connect; + this.MenuConnect.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom; this.MenuConnect.DisplayStyle = System.Windows.Forms.ToolStripItemDisplayStyle.Image; this.MenuConnect.ImageTransparentColor = System.Drawing.Color.Magenta; this.MenuConnect.Margin = new System.Windows.Forms.Padding(0); @@ -187,7 +196,6 @@ this.MenuConnect.Padding = new System.Windows.Forms.Padding(0, 0, 72, 72); this.MenuConnect.Size = new System.Drawing.Size(76, 76); this.MenuConnect.Click += new System.EventHandler(this.MenuConnect_Click); - this.MenuConnect.MouseHover += new System.EventHandler(this.MenuConnect_MouseHover); // // toolStripConnectionControl // @@ -216,6 +224,7 @@ this.FormClosing += new System.Windows.Forms.FormClosingEventHandler(this.MainV2_FormClosing); this.FormClosed += new System.Windows.Forms.FormClosedEventHandler(this.MainV2_FormClosed); this.Load += new System.EventHandler(this.MainV2_Load); + this.MouseMove += new System.Windows.Forms.MouseEventHandler(this.MainV2_MouseMove); this.Resize += new System.EventHandler(this.MainV2_Resize); this.MainMenu.ResumeLayout(false); this.MainMenu.PerformLayout(); diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 7fefa08e44..2bed4467ac 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -102,6 +102,9 @@ namespace ArdupilotMega /// used to call anything as needed. /// public static MainV2 instance = null; + + public static MainSwitcher View; + /// /// used to feed in a network link kml to the http server /// @@ -163,6 +166,8 @@ namespace ArdupilotMega InitializeComponent(); + View = MyView; + _connectionControl = toolStripConnectionControl.ConnectionControl; _connectionControl.CMB_baudrate.TextChanged += this.CMB_baudrate_TextChanged; _connectionControl.CMB_baudrate.SelectedIndexChanged += this.CMB_baudrate_SelectedIndexChanged; @@ -2050,6 +2055,12 @@ namespace ArdupilotMega /// protected override bool ProcessCmdKey(ref Message msg, Keys keyData) { + if (keyData == Keys.F12) + { + MenuConnect_Click(null, null); + return true; + } + if (keyData == (Keys.Control | Keys.F)) // temp { Form frm = new temp(); @@ -2253,9 +2264,15 @@ namespace ArdupilotMega CMB_serialport_Click(sender, e); } - private void MenuConnect_MouseHover(object sender, EventArgs e) + private void MainMenu_MouseLeave(object sender, EventArgs e) { + //MainMenu.Visible = false; + } + private void MainV2_MouseMove(object sender, MouseEventArgs e) + { + // if (e.Y < 50) + // MainMenu.Visible = true; } } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/MainV2.resx b/Tools/ArdupilotMegaPlanner/MainV2.resx index 05bc2f223b..c0e0c546ed 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.resx +++ b/Tools/ArdupilotMegaPlanner/MainV2.resx @@ -179,7 +179,7 @@ - 47 + 46 diff --git a/Tools/ArdupilotMegaPlanner/Mavlink/MAVLink.cs b/Tools/ArdupilotMegaPlanner/Mavlink/MAVLink.cs index b388500b80..c7b14873b1 100644 --- a/Tools/ArdupilotMegaPlanner/Mavlink/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/Mavlink/MAVLink.cs @@ -705,6 +705,8 @@ namespace ArdupilotMega DateTime lastmessage = DateTime.MinValue; + //hires.Stopwatch stopwatch = new hires.Stopwatch(); + do { @@ -731,11 +733,13 @@ namespace ArdupilotMega throw new Exception("Timeout on read - getParamList " + got.Count + " " + param_total + "\n\nYour serial link isn't fast enough\n"); } + //Console.WriteLine(DateTime.Now.Millisecond + " gp0 "); + byte[] buffer = readPacket(); + //Console.WriteLine(DateTime.Now.Millisecond + " gp1 "); if (buffer.Length > 5) { - //stopwatch.Reset(); - //stopwatch.Start(); + // stopwatch.Start(); if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE) { restart = DateTime.Now; @@ -763,16 +767,22 @@ namespace ArdupilotMega continue; } - log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (par.param_count) + " name: " + paramID); + //Console.WriteLine(DateTime.Now.Millisecond + " gp2 "); - //Console.WriteLine(DateTime.Now.Millisecond + " gp " + BaseStream.BytesToRead); + if (!MainV2.MONO) + log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (par.param_count) + " name: " + paramID); + + //Console.WriteLine(DateTime.Now.Millisecond + " gp2a "); modifyParamForDisplay(true, paramID, ref par.param_value); param[paramID] = (par.param_value); + + //Console.WriteLine(DateTime.Now.Millisecond + " gp2b "); + param_count++; got.Add(par.param_index); - Console.WriteLine(DateTime.Now.Millisecond + " gp1 " + BaseStream.BytesToRead); + //Console.WriteLine(DateTime.Now.Millisecond + " gp3 "); this.frmProgressReporter.UpdateProgressAndStatus((got.Count * 100) / param_total, "Got param " + paramID); @@ -785,8 +795,8 @@ namespace ArdupilotMega //Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " want " + MAVLINK_MSG_ID_PARAM_VALUE + " btr " + BaseStream.BytesToRead); } //stopwatch.Stop(); - //Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed); - Console.WriteLine(DateTime.Now.Millisecond + " gp2 " + BaseStream.BytesToRead); + // Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed); + // Console.WriteLine(DateTime.Now.Millisecond + " gp4 " + BaseStream.BytesToRead); } } while (got.Count < param_total); @@ -1777,7 +1787,7 @@ namespace ArdupilotMega /// wp no /// global or relative /// 0 = no , 2 = guided mode - public void setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current) + public MAV_MISSION_RESULT setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current) { MainV2.giveComport = true; #if MAVLINK10 @@ -1900,7 +1910,7 @@ namespace ArdupilotMega log.Info("set wp " + index + " ACK 47 : " + buffer[5] + " ans " + Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString())); - break; + return (MAV_MISSION_RESULT)ans.type; } else if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST) { @@ -1913,7 +1923,8 @@ namespace ArdupilotMega { log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]); MainV2.giveComport = false; - break; + + return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED; } else { @@ -1954,6 +1965,29 @@ namespace ArdupilotMega #endif } } + + // return MAV_MISSION_RESULT.MAV_MISSION_INVALID; + } + + public void setGuidedModeWP(Locationwp gotohere) + { + if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0) + return; + + MainV2.giveComport = true; + + try + { + gotohere.id =(byte)MAV_CMD.WAYPOINT; + + MAV_MISSION_RESULT ans = MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2); + + if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED) + throw new Exception("Guided Mode Failed"); + } + catch (Exception ex) { log.Error(ex); } + + MainV2.giveComport = false; } public void setMountConfigure(MAV_MOUNT_MODE mountmode, bool stabroll, bool stabpitch, bool stabyaw) @@ -2060,10 +2094,14 @@ namespace ArdupilotMega int readcount = 0; lastbad = new byte[2]; + byte[] headbuffer = new byte[6]; + BaseStream.ReadTimeout = 1200; // 1200 ms between chars - the gps detection requires this. DateTime start = DateTime.Now; + //Console.WriteLine(DateTime.Now.Millisecond + " SR0 " + BaseStream.BytesToRead); + try { // test fabs idea - http://diydrones.com/profiles/blogs/flying-with-joystick?commentId=705844%3AComment%3A818712&xg_source=msg_com_blogpost @@ -2078,7 +2116,7 @@ namespace ArdupilotMega lock (readlock) { -// Console.WriteLine(DateTime.Now.Millisecond + " SR " + BaseStream.BytesToRead); + //Console.WriteLine(DateTime.Now.Millisecond + " SR1 " + BaseStream.BytesToRead); while (BaseStream.IsOpen || logreadmode) { @@ -2123,22 +2161,26 @@ namespace ArdupilotMega DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout); + // Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream.BytesToRead); + while (BaseStream.BytesToRead <= 0) { if (DateTime.Now > to) { - log.InfoFormat("MAVLINK: S wait time out btr {0} len {1}", BaseStream.BytesToRead, length); + log.InfoFormat("MAVLINK: 1 wait time out btr {0} len {1}", BaseStream.BytesToRead, length); throw new Exception("Timeout"); } - // System.Threading.Thread.Sleep(1); + System.Threading.Thread.Sleep(1); + //Console.WriteLine(DateTime.Now.Millisecond + " SR0b " + BaseStream.BytesToRead); } - //Console.WriteLine(DateTime.Now.Millisecond + " SR1 " + BaseStream.BytesToRead); + //Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream.BytesToRead); if (BaseStream.IsOpen) { - buffer[count] = (byte)BaseStream.ReadByte(); + BaseStream.Read(buffer, count, 1); if (rawlogfile != null && rawlogfile.BaseStream.CanWrite) rawlogfile.Write(buffer[count]); } + //Console.WriteLine(DateTime.Now.Millisecond + " SR1b " + BaseStream.BytesToRead); } } catch (Exception e) { log.Info("MAVLink readpacket read error: " + e.ToString()); break; } @@ -2163,8 +2205,31 @@ namespace ArdupilotMega //Console.WriteLine(DateTime.Now.Millisecond + " SR2 " + BaseStream.BytesToRead); + // check for a header if (buffer[0] == 'U' || buffer[0] == 254) { + // if we have the header, and no other chars, get the length and packet identifiers + if (count == 0 && !logreadmode) + { + DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout); + + while (BaseStream.BytesToRead < 5) + { + if (DateTime.Now > to) + { + log.InfoFormat("MAVLINK: 2 wait time out btr {0} len {1}", BaseStream.BytesToRead, length); + throw new Exception("Timeout"); + } + System.Threading.Thread.Sleep(1); + //Console.WriteLine(DateTime.Now.Millisecond + " SR0b " + BaseStream.BytesToRead); + } + int read = BaseStream.Read(buffer, 1, 5); + count = read; + if (rawlogfile != null && rawlogfile.BaseStream.CanWrite) + rawlogfile.Write(buffer,1,read); + } + + // packet length length = buffer[1] + 6 + 2 - 2; // data + header + checksum - U - length if (count >= 5 || logreadmode) { @@ -2198,10 +2263,10 @@ namespace ArdupilotMega { if (DateTime.Now > to) { - log.InfoFormat("MAVLINK: L wait time out btr {0} len {1}", BaseStream.BytesToRead, length); + log.InfoFormat("MAVLINK: 3 wait time out btr {0} len {1}", BaseStream.BytesToRead, length); break; } - //Console.WriteLine("data " + 0 + " " + length + " aval " + BaseStream.BytesToRead); + System.Threading.Thread.Sleep(1); } if (BaseStream.IsOpen) { @@ -2213,7 +2278,6 @@ namespace ArdupilotMega } } } - //Console.WriteLine("data " + read + " " + length + " aval " + this.BytesToRead); count = length + 2; } catch { break; } @@ -2315,31 +2379,40 @@ namespace ArdupilotMega } else { + + byte packetSeqNo = buffer[2]; int expectedPacketSeqNo = ((recvpacketcount + 1) % 0x100); - if (packetSeqNo != expectedPacketSeqNo) + if (buffer[5] == MAVLINK_MSG_ID_SIMSTATE) { - synclost++; // actualy sync loss's - int numLost = 0; - - if (packetSeqNo < ((recvpacketcount + 1))) // recvpacketcount = 255 then 10 < 256 = true if was % 0x100 this would fail - { - numLost = 0x100 - expectedPacketSeqNo + packetSeqNo; - } - else - { - numLost = packetSeqNo - recvpacketcount; - } - packetslost += numLost; - WhenPacketLost.OnNext(numLost); - - log.InfoFormat("lost {0} pkts {1}", packetSeqNo, (int)packetslost); + // sitl injects a packet with a bad sequence number } + else + { + if (packetSeqNo != expectedPacketSeqNo) + { + synclost++; // actualy sync loss's + int numLost = 0; - packetsnotlost++; + if (packetSeqNo < ((recvpacketcount + 1))) // recvpacketcount = 255 then 10 < 256 = true if was % 0x100 this would fail + { + numLost = 0x100 - expectedPacketSeqNo + packetSeqNo; + } + else + { + numLost = packetSeqNo - recvpacketcount; + } + packetslost += numLost; + WhenPacketLost.OnNext(numLost); - recvpacketcount = packetSeqNo; + log.InfoFormat("lost {0} pkts {1}", packetSeqNo, (int)packetslost); + } + + packetsnotlost++; + + recvpacketcount = packetSeqNo; + } WhenPacketReceived.OnNext(1); // Console.WriteLine(DateTime.Now.Millisecond); } diff --git a/Tools/ArdupilotMegaPlanner/Mavlink/MAVLinkTypes.cs b/Tools/ArdupilotMegaPlanner/Mavlink/MAVLinkTypes.cs index 11ff3c7fff..762d2b4835 100644 --- a/Tools/ArdupilotMegaPlanner/Mavlink/MAVLinkTypes.cs +++ b/Tools/ArdupilotMegaPlanner/Mavlink/MAVLinkTypes.cs @@ -25,9 +25,9 @@ namespace ArdupilotMega public const bool MAVLINK_NEED_BYTE_SWAP = (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN); - public byte[] MAVLINK_MESSAGE_LENGTHS = new byte[] {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 36, 3, 9, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}; + public byte[] MAVLINK_MESSAGE_LENGTHS = new byte[] {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}; - public byte[] MAVLINK_MESSAGE_CRCS = new byte[] {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 42, 21, 21, 144, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}; + public byte[] MAVLINK_MESSAGE_CRCS = new byte[] {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 111, 21, 21, 144, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}; public Type[] MAVLINK_MESSAGE_INFO = new Type[] {typeof( mavlink_heartbeat_t ), typeof( mavlink_sys_status_t ), typeof( mavlink_system_time_t ), null, typeof( mavlink_ping_t ), typeof( mavlink_change_operator_control_t ), typeof( mavlink_change_operator_control_ack_t ), typeof( mavlink_auth_key_t ), null, null, null, typeof( mavlink_set_mode_t ), null, null, null, null, null, null, null, null, typeof( mavlink_param_request_read_t ), typeof( mavlink_param_request_list_t ), typeof( mavlink_param_value_t ), typeof( mavlink_param_set_t ), typeof( mavlink_gps_raw_int_t ), typeof( mavlink_gps_status_t ), typeof( mavlink_scaled_imu_t ), typeof( mavlink_raw_imu_t ), typeof( mavlink_raw_pressure_t ), typeof( mavlink_scaled_pressure_t ), typeof( mavlink_attitude_t ), typeof( mavlink_attitude_quaternion_t ), typeof( mavlink_local_position_ned_t ), typeof( mavlink_global_position_int_t ), typeof( mavlink_rc_channels_scaled_t ), typeof( mavlink_rc_channels_raw_t ), typeof( mavlink_servo_output_raw_t ), typeof( mavlink_mission_request_partial_list_t ), typeof( mavlink_mission_write_partial_list_t ), typeof( mavlink_mission_item_t ), typeof( mavlink_mission_request_t ), typeof( mavlink_mission_set_current_t ), typeof( mavlink_mission_current_t ), typeof( mavlink_mission_request_list_t ), typeof( mavlink_mission_count_t ), typeof( mavlink_mission_clear_all_t ), typeof( mavlink_mission_item_reached_t ), typeof( mavlink_mission_ack_t ), typeof( mavlink_set_gps_global_origin_t ), typeof( mavlink_gps_global_origin_t ), typeof( mavlink_set_local_position_setpoint_t ), typeof( mavlink_local_position_setpoint_t ), typeof( mavlink_global_position_setpoint_int_t ), typeof( mavlink_set_global_position_setpoint_int_t ), typeof( mavlink_safety_set_allowed_area_t ), typeof( mavlink_safety_allowed_area_t ), typeof( mavlink_set_roll_pitch_yaw_thrust_t ), typeof( mavlink_set_roll_pitch_yaw_speed_thrust_t ), typeof( mavlink_roll_pitch_yaw_thrust_setpoint_t ), typeof( mavlink_roll_pitch_yaw_speed_thrust_setpoint_t ), typeof( mavlink_set_quad_motors_setpoint_t ), typeof( mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t ), typeof( mavlink_nav_controller_output_t ), typeof( mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t ), typeof( mavlink_state_correction_t ), null, typeof( mavlink_request_data_stream_t ), typeof( mavlink_data_stream_t ), null, typeof( mavlink_manual_control_t ), typeof( mavlink_rc_channels_override_t ), null, null, null, typeof( mavlink_vfr_hud_t ), null, typeof( mavlink_command_long_t ), typeof( mavlink_command_ack_t ), null, null, null, null, null, null, null, null, null, null, null, typeof( mavlink_local_position_ned_system_global_offset_t ), typeof( mavlink_hil_state_t ), typeof( mavlink_hil_controls_t ), typeof( mavlink_hil_rc_inputs_raw_t ), null, null, null, null, null, null, null, typeof( mavlink_optical_flow_t ), typeof( mavlink_global_vision_position_estimate_t ), typeof( mavlink_vision_position_estimate_t ), typeof( mavlink_vision_speed_estimate_t ), typeof( mavlink_vicon_position_estimate_t ), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof( mavlink_sensor_offsets_t ), typeof( mavlink_set_mag_offsets_t ), typeof( mavlink_meminfo_t ), typeof( mavlink_ap_adc_t ), typeof( mavlink_digicam_configure_t ), typeof( mavlink_digicam_control_t ), typeof( mavlink_mount_configure_t ), typeof( mavlink_mount_control_t ), typeof( mavlink_mount_status_t ), null, typeof( mavlink_fence_point_t ), typeof( mavlink_fence_fetch_point_t ), typeof( mavlink_fence_status_t ), typeof( mavlink_ahrs_t ), typeof( mavlink_simstate_t ), typeof( mavlink_hwstatus_t ), typeof( mavlink_radio_t ), typeof( mavlink_limits_status_t ), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof( mavlink_memory_vect_t ), typeof( mavlink_debug_vect_t ), typeof( mavlink_named_value_float_t ), typeof( mavlink_named_value_int_t ), typeof( mavlink_statustext_t ), typeof( mavlink_debug_t ), null}; @@ -933,7 +933,8 @@ namespace ArdupilotMega public Single ygyro; /// Angular speed around Z axis rad/s public Single zgyro; - + public Single lat; + public Single lng; }; diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index eaf58d225c..2440a0bf00 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.1.*")] -[assembly: AssemblyFileVersion("1.2.4")] +[assembly: AssemblyFileVersion("1.2.5")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/temp.Designer.cs b/Tools/ArdupilotMegaPlanner/temp.Designer.cs index 0399dff3e9..1adfa2a45b 100644 --- a/Tools/ArdupilotMegaPlanner/temp.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/temp.Designer.cs @@ -32,8 +32,6 @@ this.label2 = new System.Windows.Forms.Label(); this.label3 = new System.Windows.Forms.Label(); this.BUT_paramgen = new ArdupilotMega.Controls.MyButton(); - this.BUT_magcalib = new ArdupilotMega.Controls.MyButton(); - this.BUT_ant_track = new ArdupilotMega.Controls.MyButton(); this.BUT_follow_me = new ArdupilotMega.Controls.MyButton(); this.BUT_georefimage = new ArdupilotMega.Controls.MyButton(); this.BUT_lang_edit = new ArdupilotMega.Controls.MyButton(); @@ -80,7 +78,7 @@ // // BUT_paramgen // - this.BUT_paramgen.Location = new System.Drawing.Point(323, 118); + this.BUT_paramgen.Location = new System.Drawing.Point(459, 185); this.BUT_paramgen.Name = "BUT_paramgen"; this.BUT_paramgen.Size = new System.Drawing.Size(75, 23); this.BUT_paramgen.TabIndex = 20; @@ -88,28 +86,9 @@ this.BUT_paramgen.UseVisualStyleBackColor = true; this.BUT_paramgen.Click += new System.EventHandler(this.BUT_paramgen_Click); // - // BUT_magcalib - // - this.BUT_magcalib.Location = new System.Drawing.Point(119, 164); - this.BUT_magcalib.Name = "BUT_magcalib"; - this.BUT_magcalib.Size = new System.Drawing.Size(96, 23); - this.BUT_magcalib.TabIndex = 19; - this.BUT_magcalib.Text = "Mag Calib"; - this.BUT_magcalib.Click += new System.EventHandler(this.BUT_magcalib_Click); - // - // BUT_ant_track - // - this.BUT_ant_track.Location = new System.Drawing.Point(404, 164); - this.BUT_ant_track.Name = "BUT_ant_track"; - this.BUT_ant_track.Size = new System.Drawing.Size(75, 23); - this.BUT_ant_track.TabIndex = 18; - this.BUT_ant_track.Text = "Antenna Tracker"; - this.BUT_ant_track.UseVisualStyleBackColor = true; - this.BUT_ant_track.Click += new System.EventHandler(this.BUT_ant_track_Click); - // // BUT_follow_me // - this.BUT_follow_me.Location = new System.Drawing.Point(485, 164); + this.BUT_follow_me.Location = new System.Drawing.Point(378, 185); this.BUT_follow_me.Name = "BUT_follow_me"; this.BUT_follow_me.Size = new System.Drawing.Size(75, 23); this.BUT_follow_me.TabIndex = 17; @@ -119,7 +98,7 @@ // // BUT_georefimage // - this.BUT_georefimage.Location = new System.Drawing.Point(221, 164); + this.BUT_georefimage.Location = new System.Drawing.Point(195, 185); this.BUT_georefimage.Name = "BUT_georefimage"; this.BUT_georefimage.Size = new System.Drawing.Size(96, 23); this.BUT_georefimage.TabIndex = 0; @@ -128,7 +107,7 @@ // // BUT_lang_edit // - this.BUT_lang_edit.Location = new System.Drawing.Point(323, 164); + this.BUT_lang_edit.Location = new System.Drawing.Point(297, 185); this.BUT_lang_edit.Name = "BUT_lang_edit"; this.BUT_lang_edit.Size = new System.Drawing.Size(75, 23); this.BUT_lang_edit.TabIndex = 16; @@ -261,8 +240,6 @@ this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; this.ClientSize = new System.Drawing.Size(685, 285); this.Controls.Add(this.BUT_paramgen); - this.Controls.Add(this.BUT_magcalib); - this.Controls.Add(this.BUT_ant_track); this.Controls.Add(this.BUT_follow_me); this.Controls.Add(this.BUT_georefimage); this.Controls.Add(this.BUT_lang_edit); @@ -309,8 +286,6 @@ private ArdupilotMega.Controls.MyButton BUT_lang_edit; private ArdupilotMega.Controls.MyButton BUT_georefimage; private ArdupilotMega.Controls.MyButton BUT_follow_me; - private ArdupilotMega.Controls.MyButton BUT_ant_track; - private ArdupilotMega.Controls.MyButton BUT_magcalib; private Controls.MyButton BUT_paramgen; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/temp.cs b/Tools/ArdupilotMegaPlanner/temp.cs index 643e9a17b1..a7f750f1cf 100644 --- a/Tools/ArdupilotMegaPlanner/temp.cs +++ b/Tools/ArdupilotMegaPlanner/temp.cs @@ -895,19 +895,11 @@ namespace ArdupilotMega si.Show(); } - private void BUT_ant_track_Click(object sender, EventArgs e) - { - new Antenna.Tracker().Show(); - } - - private void BUT_magcalib_Click(object sender, EventArgs e) - { - MagCalib.ProcessLog(); - } - private void BUT_paramgen_Click(object sender, EventArgs e) { ParameterMetaDataParser.GetParameterInformation(); } + + } }