diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
index a0ee6c833b..e00cfbd8f9 100644
--- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
+++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
@@ -242,11 +242,14 @@
- UserControl
+ Component
ImageLabel.cs
+
+ UserControl
+
Form
diff --git a/Tools/ArdupilotMegaPlanner/Controls/ImageLabel.cs b/Tools/ArdupilotMegaPlanner/Controls/ImageLabel.cs
index 2e9637b0d1..a5524d2cd4 100644
--- a/Tools/ArdupilotMegaPlanner/Controls/ImageLabel.cs
+++ b/Tools/ArdupilotMegaPlanner/Controls/ImageLabel.cs
@@ -9,7 +9,7 @@ using System.Windows.Forms;
namespace ArdupilotMega
{
- public partial class ImageLabel : UserControl
+ public partial class ImageLabel : ContainerControl
{
public new event EventHandler Click;
diff --git a/Tools/ArdupilotMegaPlanner/Controls/myGMAP.cs b/Tools/ArdupilotMegaPlanner/Controls/myGMAP.cs
new file mode 100644
index 0000000000..8af08373f7
--- /dev/null
+++ b/Tools/ArdupilotMegaPlanner/Controls/myGMAP.cs
@@ -0,0 +1,39 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+namespace ArdupilotMega
+{
+ class myGMAP : GMap.NET.WindowsForms.GMapControl
+ {
+ public bool inOnPaint = false;
+
+ protected override void OnPaint(System.Windows.Forms.PaintEventArgs e)
+ {
+ if (inOnPaint)
+ {
+ Console.WriteLine("Was in onpaint Gmap th:" + System.Threading.Thread.CurrentThread.Name);
+ return;
+ }
+ inOnPaint = true;
+
+ try
+ {
+ base.OnPaint(e);
+ }
+ catch (Exception ex) { Console.WriteLine(ex.ToString()); }
+
+ inOnPaint = false;
+ }
+
+ protected override void OnMouseMove(System.Windows.Forms.MouseEventArgs e)
+ {
+ try
+ {
+ base.OnMouseMove(e);
+ }
+ catch (Exception ex) { Console.WriteLine(ex.ToString()); }
+ }
+ }
+}
diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs
index 57709156c8..fa10636601 100644
--- a/Tools/ArdupilotMegaPlanner/CurrentState.cs
+++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs
@@ -309,12 +309,18 @@ namespace ArdupilotMega
if (ind != -1)
logdata = logdata.Substring(0, ind);
- while (messages.Count > 5)
+ try
{
- messages.RemoveAt(0);
- }
+ while (messages.Count > 5)
+ {
+ messages.RemoveAt(0);
+ }
+
messages.Add(logdata + "\n");
+ }
+ catch { }
+
mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null;
}
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs
index b9ecf16a80..b125db82f4 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs
@@ -55,7 +55,7 @@
this.zg1 = new ZedGraph.ZedGraphControl();
this.lbl_winddir = new ArdupilotMega.MyLabel();
this.lbl_windvel = new ArdupilotMega.MyLabel();
- this.gMapControl1 = new GMap.NET.WindowsForms.GMapControl();
+ this.gMapControl1 = new myGMAP();
this.panel1 = new System.Windows.Forms.Panel();
this.TXT_lat = new ArdupilotMega.MyLabel();
this.Zoomlevel = new System.Windows.Forms.NumericUpDown();
@@ -1320,7 +1320,7 @@
private ArdupilotMega.MyLabel TXT_long;
private ArdupilotMega.MyLabel TXT_alt;
private System.Windows.Forms.CheckBox CHK_autopan;
- private GMap.NET.WindowsForms.GMapControl gMapControl1;
+ private myGMAP gMapControl1;
private ZedGraph.ZedGraphControl zg1;
private System.Windows.Forms.TabControl tabControl1;
private System.Windows.Forms.TabPage tabGauges;
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
index 31a96a2db7..5094e64519 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
@@ -369,7 +369,7 @@ namespace ArdupilotMega.GCSViews
try
{
//Console.WriteLine(DateTime.Now.Millisecond);
- MainV2.cs.UpdateCurrentSettings(bindingSource1);
+ updateBindingSource();
//Console.WriteLine(DateTime.Now.Millisecond + " done ");
if (tunning.AddMilliseconds(50) < DateTime.Now && CB_tuning.Checked == true)
@@ -402,6 +402,11 @@ namespace ArdupilotMega.GCSViews
{
gMapControl1.HoldInvalidation = true;
+ while (gMapControl1.inOnPaint == true)
+ {
+ System.Threading.Thread.Sleep(1);
+ }
+
if (trackPoints.Count > int.Parse(MainV2.config["NUM_tracklength"].ToString()))
{
trackPoints.RemoveRange(0, trackPoints.Count - int.Parse(MainV2.config["NUM_tracklength"].ToString()));
@@ -500,6 +505,14 @@ namespace ArdupilotMega.GCSViews
Console.WriteLine("FD Main loop exit");
}
+ private void updateBindingSource()
+ {
+ this.Invoke((System.Windows.Forms.MethodInvoker)delegate()
+ {
+ MainV2.cs.UpdateCurrentSettings(bindingSource1);
+ });
+ }
+
private void updateMapPosition(PointLatLng currentloc)
{
this.BeginInvoke((MethodInvoker)delegate()
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs
index d4b7fcea02..4f77a22a7e 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs
@@ -101,7 +101,7 @@
this.lbl_distance = new System.Windows.Forms.Label();
this.lbl_homedist = new System.Windows.Forms.Label();
this.lbl_prevdist = new System.Windows.Forms.Label();
- this.MainMap = new GMap.NET.WindowsForms.GMapControl();
+ this.MainMap = new myGMAP();
this.contextMenuStrip1 = new System.Windows.Forms.ContextMenuStrip(this.components);
this.deleteWPToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
this.loiterToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
@@ -979,7 +979,7 @@
private BSE.Windows.Forms.Panel panelWaypoints;
private BSE.Windows.Forms.Panel panelAction;
private System.Windows.Forms.Panel panelMap;
- private GMap.NET.WindowsForms.GMapControl MainMap;
+ private myGMAP MainMap;
private MyTrackBar trackBar1;
private System.Windows.Forms.Label label11;
private System.Windows.Forms.Label lbl_distance;
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
index 50a4a64515..8c408cb277 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
@@ -81,6 +81,31 @@ namespace ArdupilotMega.GCSViews
public uint magic;
}
+ [StructLayout(LayoutKind.Sequential, Pack = 1)]
+ public struct sitldata
+ {
+ public double lat;
+ public double lon;
+ public double alt;
+ public double heading;
+ public double v_north;
+ public double v_east;
+ public double ax;
+ public double ay;
+ public double az;
+ public double phidot;
+ public double thetadot;
+ public double psidot;
+ public double phi;
+ public double theta;
+ ///
+ /// heading
+ ///
+ public double psi;
+ public double vcas;
+ public int check;
+ }
+
const int AEROSIMRC_MAX_CHANNELS = 39;
//-----------------------------------------------------------------------------
@@ -263,6 +288,11 @@ namespace ArdupilotMega.GCSViews
SetupUDPRecv();
+ if (chkSensor.Checked)
+ {
+ SITLSEND = new UdpClient(simIP, 5501);
+ }
+
if (RAD_softXplanes.Checked)
{
SetupUDPXplanes();
@@ -286,12 +316,9 @@ namespace ArdupilotMega.GCSViews
System.Threading.Thread.Sleep(2000);
- SITLSEND = new UdpClient(simIP, 5501);
-
SetupTcpJSBSim(); // old style
}
-
SetupUDPXplanes(); // fg udp style
SetupUDPMavLink(); // pass traffic - raw
}
@@ -545,7 +572,7 @@ namespace ArdupilotMega.GCSViews
Console.WriteLine("REQ streams - sim");
try
{
- if (CHK_quad.Checked && !RAD_aerosimrc.Checked)
+ if (CHK_quad.Checked && !RAD_aerosimrc.Checked)// || chkSensor.Checked && RAD_JSBSim.Checked)
{
comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RAW_CONTROLLER, 0); // request servoout
}
@@ -567,6 +594,8 @@ namespace ArdupilotMega.GCSViews
int recv = SimulatorRECV.ReceiveFrom(udpdata, ref Remote);
RECVprocess(udpdata, recv, comPort);
+
+ hzcount++;
}
}
catch
@@ -598,7 +627,7 @@ namespace ArdupilotMega.GCSViews
if ((DateTime.Now - simsendtime).TotalMilliseconds > 19)
{
- hzcount++;
+ //hzcount++;
simsendtime = DateTime.Now;
processArduPilot();
}
@@ -607,7 +636,7 @@ namespace ArdupilotMega.GCSViews
if (hzcounttime.Second != DateTime.Now.Second)
{
- // Console.WriteLine("SIM hz {0}", hzcount);
+ Console.WriteLine("SIM hz {0}", hzcount);
hzcount = 0;
hzcounttime = DateTime.Now;
}
@@ -758,26 +787,13 @@ namespace ArdupilotMega.GCSViews
float rdiff = (float)((att.roll - oldatt.roll) / timediff.TotalSeconds);
float ydiff = (float)((att.yaw - oldatt.yaw) / timediff.TotalSeconds);
- //Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]);
+// Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]);
oldatt = att;
- if (xplane9)
- {
- rdiff = DATA[17][1];
- pdiff = DATA[17][0];
- ydiff = DATA[17][2];
- }
- else
- {
- rdiff = DATA[16][1];
- pdiff = DATA[16][0];
- ydiff = DATA[16][2];
- }
-
- Int16 xgyro = Constrain(rdiff * 1000.0, Int16.MinValue, Int16.MaxValue);
- Int16 ygyro = Constrain(pdiff * 1000.0, Int16.MinValue, Int16.MaxValue);
- Int16 zgyro = Constrain(ydiff * 1000.0, Int16.MinValue, Int16.MaxValue);
+ Int16 xgyro = Constrain(att.rollspeed * 1000.0, Int16.MinValue, Int16.MaxValue);
+ Int16 ygyro = Constrain(att.pitchspeed * 1000.0, Int16.MinValue, Int16.MaxValue);
+ Int16 zgyro = Constrain(att.yawspeed * 1000.0, Int16.MinValue, Int16.MaxValue);
oldtime = DateTime.Now;
@@ -849,7 +865,7 @@ namespace ArdupilotMega.GCSViews
gps.v = ((float)(DATA[3][7] * 0.44704));
#endif
- asp.airspeed = ((float)(DATA[3][6] * 0.44704));
+ asp.airspeed = ((float)(DATA[3][5] * 0.44704));
}
@@ -1160,6 +1176,50 @@ namespace ArdupilotMega.GCSViews
return;
}
+ if (RAD_softXplanes.Checked && chkSensor.Checked)
+ {
+ sitldata sitlout = new sitldata();
+
+ ArdupilotMega.HIL.Utils.FLIGHTtoBCBF(ref att.pitchspeed, ref att.rollspeed, ref att.yawspeed, DATA[19][0] * deg2rad, DATA[19][1] * deg2rad);
+
+ //Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", att.pitchspeed, att.rollspeed, att.yawspeed, DATA[17][0], DATA[17][1], DATA[17][2]);
+
+ Tuple ans = ArdupilotMega.HIL.Utils.OGLtoBCBF(att.pitch, att.roll, att.yaw, 0, 0, 9.8);
+
+ //Console.WriteLine("acc {0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", ans.Item1, ans.Item2, ans.Item3, accel3D.X, accel3D.Y, accel3D.Z);
+
+ sitlout.alt = gps.alt;
+ sitlout.lat = gps.lat;
+ sitlout.lon = gps.lon;
+ sitlout.heading = gps.hdg;
+
+ sitlout.v_north = DATA[21][4];
+ sitlout.v_east = DATA[21][5];
+
+ // correct accel
+ sitlout.ax = -ans.Item2; // pitch
+ sitlout.ay = -ans.Item1; // roll
+ sitlout.az = ans.Item3; // yaw
+
+ sitlout.phidot = -0.5;// att.pitchspeed;
+// sitlout.thetadot = att.rollspeed;
+ //sitlout.psidot = att.yawspeed;
+
+ sitlout.phi = att.roll * rad2deg;
+ sitlout.theta = att.pitch * rad2deg;
+ sitlout.psi = att.yaw * rad2deg;
+
+ sitlout.vcas = asp.airspeed;
+
+ sitlout.check = (int)0x4c56414e;
+
+ byte[] sendme = StructureToByteArray(sitlout);
+
+ SITLSEND.Send(sendme,sendme.Length);
+
+ return;
+ }
+
#if MAVLINK10
TimeSpan gpsspan = DateTime.Now - lastgpsupdate;
@@ -1624,6 +1684,25 @@ namespace ArdupilotMega.GCSViews
}
}
+ byte[] StructureToByteArray(object obj)
+ {
+
+ int len = Marshal.SizeOf(obj);
+
+ byte[] arr = new byte[len];
+
+ IntPtr ptr = Marshal.AllocHGlobal(len);
+
+ Marshal.StructureToPtr(obj, ptr, true);
+
+ Marshal.Copy(ptr, arr, 0, len);
+
+ Marshal.FreeHGlobal(ptr);
+
+ return arr;
+
+ }
+
private void RAD_softXplanes_CheckedChanged(object sender, EventArgs e)
{
diff --git a/Tools/ArdupilotMegaPlanner/HIL/Utils.cs b/Tools/ArdupilotMegaPlanner/HIL/Utils.cs
index 4d55cd86c3..c3522d9b2a 100644
--- a/Tools/ArdupilotMegaPlanner/HIL/Utils.cs
+++ b/Tools/ArdupilotMegaPlanner/HIL/Utils.cs
@@ -175,19 +175,19 @@ namespace ArdupilotMega.HIL
// The +Y axis points straight up away from the center of the earth at the reference point.
// First we shall convert from this East Up South frame, to a more conventional NED (North East Down) frame.
- x_NED = radians(x) * -1.0;
- y_NED = radians(y) * 1.0;
- z_NED = radians(z) * -1.0;
+ x_NED = (x) * -1.0;
+ y_NED = (y) * 1.0;
+ z_NED = (z) * -1.0;
// Next calculate cos & sin of angles for use in the transformation matrix.
// r, p & y subscripts stand for roll pitch and yaw.
- Cr = Math.Cos(radians(phi));
- Cp = Math.Cos(radians(theta));
- Cy = Math.Cos(radians(psi));
- Sr = Math.Sin(radians(phi));
- Sp = Math.Sin(radians(theta));
- Sy = Math.Sin(radians(psi));
+ Cr = Math.Cos((phi));
+ Cp = Math.Cos((theta));
+ Cy = Math.Cos((psi));
+ Sr = Math.Sin((phi));
+ Sp = Math.Sin((theta));
+ Sy = Math.Sin((psi));
// Next we need to rotate our accelerations from the NED reference frame, into the body fixed reference frame
@@ -202,7 +202,7 @@ namespace ArdupilotMega.HIL
y = (x_NED * ((Sr * Sp * Cy) - (Cr * Sy))) + (y_NED * ((Sr * Sp * Sy) + (Cr * Cy))) + (z_NED * Sr * Cp);
z = (x_NED * ((Cr * Sp * Cy) + (Sr * Sy))) + (y_NED * ((Cr * Sp * Sy) - (Sr * Cy))) + (z_NED * Cr * Cp);
- return new Tuple(degrees(x), degrees(y), degrees(z));
+ return new Tuple((x), (y), (z));
}
diff --git a/Tools/ArdupilotMegaPlanner/HUD.cs b/Tools/ArdupilotMegaPlanner/HUD.cs
index cce7c32b16..2b01f3242d 100644
--- a/Tools/ArdupilotMegaPlanner/HUD.cs
+++ b/Tools/ArdupilotMegaPlanner/HUD.cs
@@ -240,6 +240,8 @@ namespace hud
started = true;
}
+ bool inOnPaint = false;
+
protected override void OnPaint(PaintEventArgs e)
{
//GL.Enable(EnableCap.AlphaTest)
@@ -261,6 +263,14 @@ namespace hud
return;
}
+ if (inOnPaint)
+ {
+ Console.WriteLine("Was in onpaint Hud th:" + System.Threading.Thread.CurrentThread.Name);
+ return;
+ }
+
+ inOnPaint = true;
+
starttime = DateTime.Now;
try
@@ -284,6 +294,8 @@ namespace hud
}
catch (Exception ex) { Console.WriteLine(ex.ToString()); }
+ inOnPaint = false;
+
count++;
huddrawtime += (int)(DateTime.Now - starttime).TotalMilliseconds;
@@ -1479,7 +1491,7 @@ namespace hud
if (e == null || P == null || pth == null || pth.PointCount == 0)
return;
- if (!ArdupilotMega.MainV2.MONO)
+ //if (!ArdupilotMega.MainV2.MONO)
e.DrawPath(P, pth);
//Draw the face
diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs
index eeab0ed1c8..ffbb76efdf 100644
--- a/Tools/ArdupilotMegaPlanner/MainV2.cs
+++ b/Tools/ArdupilotMegaPlanner/MainV2.cs
@@ -233,9 +233,9 @@ namespace ArdupilotMega
double Framework = Convert.ToDouble(version_names[version_names.Length - 1].Remove(0, 1), CultureInfo.InvariantCulture);
int SP = Convert.ToInt32(installed_versions.OpenSubKey(version_names[version_names.Length - 1]).GetValue("SP", 0));
- if (Framework < 4.0)
+ if (Framework < 3.5)
{
- MessageBox.Show("This program requires .NET Framework 4.0 +. You currently have " + Framework);
+ MessageBox.Show("This program requires .NET Framework 3.5. You currently have " + Framework);
}
}
diff --git a/Tools/ArdupilotMegaPlanner/Program.cs b/Tools/ArdupilotMegaPlanner/Program.cs
index 9cd29cbabe..58c39688c0 100644
--- a/Tools/ArdupilotMegaPlanner/Program.cs
+++ b/Tools/ArdupilotMegaPlanner/Program.cs
@@ -33,6 +33,8 @@ namespace ArdupilotMega
Application.SetCompatibleTextRenderingDefault(false);
try
{
+ System.Threading.Thread.CurrentThread.Name = "Base Thread";
+
Application.Run(new MainV2());
}
catch (Exception ex) { Console.WriteLine(ex.ToString()); }
diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
index 7b31841394..3a28cbf8cf 100644
--- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
+++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
@@ -34,5 +34,5 @@ using System.Resources;
// by using the '*' as shown below:
// [assembly: AssemblyVersion("1.0.*")]
[assembly: AssemblyVersion("1.0.0.0")]
-[assembly: AssemblyFileVersion("1.1.36")]
+[assembly: AssemblyFileVersion("1.1.37")]
[assembly: NeutralResourcesLanguageAttribute("")]
diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb
index 6046b97ded..11b35bd52b 100644
Binary files a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb and b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb differ