diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
index 761ff0cb1c..c1c1176ee0 100644
--- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
+++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
@@ -581,6 +581,7 @@
Always
+ Designer
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs
index bddd7f9853..a5650efb77 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs
@@ -22,6 +22,7 @@
this.bindingSource1 = new System.Windows.Forms.BindingSource(this.components);
this.tabControl1 = new System.Windows.Forms.TabControl();
this.tabActions = new System.Windows.Forms.TabPage();
+ this.BUT_script = new ArdupilotMega.MyButton();
this.BUT_joystick = new ArdupilotMega.MyButton();
this.BUT_quickmanual = new ArdupilotMega.MyButton();
this.BUT_quickrtl = new ArdupilotMega.MyButton();
@@ -236,6 +237,7 @@
//
// tabActions
//
+ this.tabActions.Controls.Add(this.BUT_script);
this.tabActions.Controls.Add(this.BUT_joystick);
this.tabActions.Controls.Add(this.BUT_quickmanual);
this.tabActions.Controls.Add(this.BUT_quickrtl);
@@ -254,6 +256,13 @@
this.tabActions.Name = "tabActions";
this.tabActions.UseVisualStyleBackColor = true;
//
+ // BUT_script
+ //
+ resources.ApplyResources(this.BUT_script, "BUT_script");
+ this.BUT_script.Name = "BUT_script";
+ this.BUT_script.UseVisualStyleBackColor = true;
+ this.BUT_script.Click += new System.EventHandler(this.BUT_script_Click);
+ //
// BUT_joystick
//
resources.ApplyResources(this.BUT_joystick, "BUT_joystick");
@@ -1331,5 +1340,6 @@
private MyLabel lbl_logpercent;
private System.Windows.Forms.ToolStripMenuItem pointCameraHereToolStripMenuItem;
private System.Windows.Forms.SplitContainer splitContainer1;
+ private MyButton BUT_script;
}
}
\ No newline at end of file
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
index 23e320a9e3..34b0c68756 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
@@ -1539,5 +1539,101 @@ namespace ArdupilotMega.GCSViews
MainV2.comPort.setMountControl(gotolocation.Lat, gotolocation.Lng, (int)(intalt / MainV2.cs.multiplierdist), true);
}
+
+ private void BUT_script_Click(object sender, EventArgs e)
+ {
+
+ System.Threading.Thread t11 = new System.Threading.Thread(new System.Threading.ThreadStart(ScriptStart))
+ {
+ IsBackground = true,
+ Name = "Script Thread"
+ };
+ t11.Start();
+ }
+
+ void ScriptStart()
+ {
+ string myscript = @"
+print 'Start Script'
+for chan in range(1,9):
+ Script.SendRC(chan,1500,False)
+Script.SendRC(3,Script.GetParam('RC3_MIN'),True)
+
+Script.Sleep(5000)
+while cs.lat == 0:
+ print 'Waiting for GPS'
+ Script.Sleep(1000)
+print 'Got GPS'
+jo = 10 * 13
+print jo
+Script.SendRC(3,1000,False)
+Script.SendRC(4,2000,True)
+cs.messages.Clear()
+Script.WaitFor('ARMING MOTORS',30000)
+Script.SendRC(4,1500,True)
+print 'Motors Armed!'
+
+Script.SendRC(3,1700,True)
+while cs.alt < 50:
+ Script.Sleep(50)
+
+Script.SendRC(5,2000,True) # acro
+
+Script.SendRC(1,2000,False) # roll
+Script.SendRC(3,1370,True) # throttle
+while cs.roll > -45: # top hald 0 - 180
+ Script.Sleep(5)
+while cs.roll < -45: # -180 - -45
+ Script.Sleep(5)
+
+Script.SendRC(5,1500,False) # stabalise
+Script.SendRC(1,1500,True) # level roll
+Script.Sleep(2000) # 2 sec to stabalise
+Script.SendRC(3,1300,True) # throttle back to land
+
+thro = 1350 # will decend
+
+while cs.alt > 0.1:
+ Script.Sleep(300)
+
+Script.SendRC(3,1000,False)
+Script.SendRC(4,1000,True)
+Script.WaitFor('DISARMING MOTORS',30000)
+Script.SendRC(4,1500,True)
+
+print 'Roll complete'
+
+";
+
+ MessageBox.Show("This is Very ALPHA");
+
+ Form scriptedit = new Form();
+
+ scriptedit.Size = new System.Drawing.Size(500,500);
+
+ TextBox tb = new TextBox();
+
+ tb.Dock = DockStyle.Fill;
+
+ tb.ScrollBars = ScrollBars.Both;
+ tb.Multiline = true;
+
+ tb.Location = new Point(0,0);
+ tb.Size = new System.Drawing.Size(scriptedit.Size.Width-30,scriptedit.Size.Height-30);
+
+ scriptedit.Controls.Add(tb);
+
+ tb.Text = myscript;
+
+ scriptedit.ShowDialog();
+
+ if (DialogResult.Yes == MessageBox.Show("Run Script", "Run this script?", MessageBoxButtons.YesNo))
+ {
+
+ Script scr = new Script();
+
+ scr.runScript(tb.Text);
+ }
+ }
}
}
\ No newline at end of file
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx
index 845fa8bfd8..6eb95e738d 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx
@@ -208,7 +208,7 @@
hud1
- hud.HUD, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ hud.HUD, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
SubMainLeft.Panel1
@@ -228,6 +228,33 @@
0
+
+ NoControl
+
+
+ 17, 93
+
+
+ 66, 23
+
+
+ 78
+
+
+ Script
+
+
+ BUT_script
+
+
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
+
+
+ tabActions
+
+
+ 0
+
NoControl
@@ -253,13 +280,13 @@
BUT_joystick
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabActions
- 0
+ 1
NoControl
@@ -283,13 +310,13 @@
BUT_quickmanual
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabActions
- 1
+ 2
NoControl
@@ -313,13 +340,13 @@
BUT_quickrtl
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabActions
- 2
+ 3
NoControl
@@ -343,13 +370,13 @@
BUT_quickauto
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabActions
- 3
+ 4
0 (Home)
@@ -373,7 +400,7 @@
tabActions
- 4
+ 5
NoControl
@@ -397,13 +424,13 @@
BUT_setwp
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabActions
- 5
+ 6
4, 64
@@ -424,7 +451,7 @@
tabActions
- 6
+ 7
NoControl
@@ -448,13 +475,13 @@
BUT_setmode
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabActions
- 7
+ 8
NoControl
@@ -478,13 +505,13 @@
BUT_clear_track
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabActions
- 8
+ 9
4, 6
@@ -505,7 +532,7 @@
tabActions
- 9
+ 10
NoControl
@@ -529,13 +556,13 @@
BUT_Homealt
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabActions
- 10
+ 11
NoControl
@@ -559,13 +586,13 @@
BUT_RAWSensor
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabActions
- 11
+ 12
NoControl
@@ -589,13 +616,13 @@
BUTrestartmission
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabActions
- 12
+ 13
NoControl
@@ -619,13 +646,13 @@
BUTactiondo
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabActions
- 13
+ 14
4, 22
@@ -673,7 +700,7 @@
Gvspeed
- AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabGauges
@@ -703,7 +730,7 @@
Gheading
- AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabGauges
@@ -733,7 +760,7 @@
Galt
- AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabGauges
@@ -766,7 +793,7 @@
Gspeed
- AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabGauges
@@ -847,7 +874,7 @@
lbl_logpercent
- ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabTLogs
@@ -898,7 +925,7 @@
BUT_log2kml
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabTLogs
@@ -949,7 +976,7 @@
BUT_playlog
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabTLogs
@@ -976,7 +1003,7 @@
BUT_loadtelem
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
tabTLogs
@@ -1162,7 +1189,7 @@
lbl_winddir
- ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
splitContainer1.Panel2
@@ -1192,7 +1219,7 @@
lbl_windvel
- ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
splitContainer1.Panel2
@@ -1427,7 +1454,7 @@
TXT_lat
- ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
panel1
@@ -1484,7 +1511,7 @@
label1
- ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
panel1
@@ -1514,7 +1541,7 @@
TXT_long
- ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
panel1
@@ -1544,7 +1571,7 @@
TXT_alt
- ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
panel1
@@ -1745,7 +1772,7 @@
label6
- ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
$this
@@ -1823,6 +1850,6 @@
FlightData
- System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
\ No newline at end of file
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs
index 8f65838cdf..55f50b788b 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs
@@ -342,7 +342,7 @@ namespace ArdupilotMega.GCSViews
{
TXT_mouselat.Text = lat.ToString();
TXT_mouselong.Text = lng.ToString();
- TXT_mousealt.Text = srtm.getAltitude(lat, lng).ToString("0");
+ TXT_mousealt.Text = srtm.getAltitude(lat, lng, MainMap.Zoom).ToString("0");
try
{
@@ -2754,16 +2754,18 @@ namespace ArdupilotMega.GCSViews
private void BUT_zoomto_Click(object sender, EventArgs e)
{
string place = "Perth, Australia";
- Common.InputBox("Location", "Enter your location", ref place);
+ if (DialogResult.OK == Common.InputBox("Location", "Enter your location", ref place))
+ {
- GeoCoderStatusCode status = MainMap.SetCurrentPositionByKeywords(place);
- if (status != GeoCoderStatusCode.G_GEO_SUCCESS)
- {
- MessageBox.Show("Google Maps Geocoder can't find: '" + place + "', reason: " + status.ToString(), "GMap.NET", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
- }
- else
- {
- MainMap.Zoom = 15;
+ GeoCoderStatusCode status = MainMap.SetCurrentPositionByKeywords(place);
+ if (status != GeoCoderStatusCode.G_GEO_SUCCESS)
+ {
+ MessageBox.Show("Google Maps Geocoder can't find: '" + place + "', reason: " + status.ToString(), "GMap.NET", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
+ }
+ else
+ {
+ MainMap.Zoom = 15;
+ }
}
}
}
diff --git a/Tools/ArdupilotMegaPlanner/HUD.cs b/Tools/ArdupilotMegaPlanner/HUD.cs
index f22114f12b..58515757a5 100644
--- a/Tools/ArdupilotMegaPlanner/HUD.cs
+++ b/Tools/ArdupilotMegaPlanner/HUD.cs
@@ -264,7 +264,7 @@ namespace hud
if (DateTime.Now.Second != countdate.Second)
{
countdate = DateTime.Now;
- Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count) + " gl " + opengl);
+ //Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count) + " gl " + opengl);
count = 0;
huddrawtime = 0;
}
diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs
index 5288c28de1..c14c2567fe 100644
--- a/Tools/ArdupilotMegaPlanner/MainV2.cs
+++ b/Tools/ArdupilotMegaPlanner/MainV2.cs
@@ -21,7 +21,6 @@ using System.Threading;
using System.Net.Sockets;
using IronPython.Hosting;
-
namespace ArdupilotMega
{
public partial class MainV2 : Form
@@ -109,15 +108,10 @@ namespace ArdupilotMega
//return;
- var engine = Python.CreateEngine();
- var scope = engine.CreateScope();
- scope.SetVariable("MainV2",this);
- Console.WriteLine(DateTime.Now.Millisecond);
- engine.CreateScriptSourceFromString("print 'hello world from python'").Execute(scope);
- Console.WriteLine(DateTime.Now.Millisecond);
- engine.CreateScriptSourceFromString("MainV2.testpython()").Execute(scope);
- Console.WriteLine(DateTime.Now.Millisecond);
+ // preload
+ Python.CreateEngine();
+
var t = Type.GetType("Mono.Runtime");
MONO = (t != null);
@@ -678,7 +672,7 @@ namespace ArdupilotMega
comPort.logfile = new BinaryWriter(File.Open(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd hh-mm-ss") + ".tlog", FileMode.CreateNew));
}
catch { MessageBox.Show("Failed to create log - wont log this session"); } // soft fail
-
+
comPort.BaseStream.PortName = CMB_serialport.Text;
comPort.Open(true);
@@ -1100,7 +1094,7 @@ namespace ArdupilotMega
if (heatbeatsend.Second != DateTime.Now.Second)
{
- Console.WriteLine("remote lost {0}", cs.packetdropremote);
+// Console.WriteLine("remote lost {0}", cs.packetdropremote);
MAVLink.__mavlink_heartbeat_t htb = new MAVLink.__mavlink_heartbeat_t();
diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
index 41b0f8de1a..4c387fa873 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.0.98")]
+[assembly: AssemblyFileVersion("1.0.99")]
[assembly: NeutralResourcesLanguageAttribute("")]
diff --git a/Tools/ArdupilotMegaPlanner/Script.cs b/Tools/ArdupilotMegaPlanner/Script.cs
index d18b5c9aac..534b3aa678 100644
--- a/Tools/ArdupilotMegaPlanner/Script.cs
+++ b/Tools/ArdupilotMegaPlanner/Script.cs
@@ -2,6 +2,7 @@
using System.Collections.Generic;
using System.Linq;
using System.Text;
+using IronPython.Hosting;
namespace ArdupilotMega
{
@@ -9,12 +10,27 @@ namespace ArdupilotMega
{
DateTime timeout = DateTime.Now;
List items = new List();
+ Microsoft.Scripting.Hosting.ScriptEngine engine;
+ Microsoft.Scripting.Hosting.ScriptScope scope;
// keeps history
MAVLink.__mavlink_rc_channels_override_t rc = new MAVLink.__mavlink_rc_channels_override_t();
public Script()
{
+ engine = Python.CreateEngine();
+ scope = engine.CreateScope();
+
+ scope.SetVariable("cs", MainV2.cs);
+ scope.SetVariable("Script", this);
+
+ Console.WriteLine(DateTime.Now.Millisecond);
+ engine.CreateScriptSourceFromString("print 'hello world from python'").Execute(scope);
+ Console.WriteLine(DateTime.Now.Millisecond);
+ engine.CreateScriptSourceFromString("print cs.roll").Execute(scope);
+ Console.WriteLine(DateTime.Now.Millisecond);
+
+
object thisBoxed = MainV2.cs;
Type test = thisBoxed.GetType();
@@ -33,7 +49,24 @@ namespace ArdupilotMega
items.Add(field.Name);
}
- }
+ }
+
+
+ public void Sleep(int ms) {
+ System.Threading.Thread.Sleep(ms);
+ }
+
+ public void runScript(string script)
+ {
+ try
+ {
+ engine.CreateScriptSourceFromString(script).Execute(scope);
+ }
+ catch (Exception e)
+ {
+ System.Windows.Forms.MessageBox.Show("Error running script " + e.Message);
+ }
+ }
public enum Conditional
{
@@ -51,36 +84,34 @@ namespace ArdupilotMega
return MainV2.comPort.setParam(param, value);
}
+ public float GetParam(string param)
+ {
+ if (MainV2.comPort.param[param] != null)
+ return (float)MainV2.comPort.param[param];
+
+ return 0.0f;
+ }
+
public bool ChangeMode(string mode)
{
MainV2.comPort.setMode(mode);
return true;
}
- public bool WaitFor(string message)
+ public bool WaitFor(string message, int timeout)
{
- while (MainV2.cs.message != message) {
+ int timein = 0;
+ while (!MainV2.cs.message.Contains(message)) {
System.Threading.Thread.Sleep(5);
+ timein += 5;
+ if (timein > timeout)
+ return false;
}
return true;
}
-
- public bool WaitFor(string item, Conditional cond,double value ,int timeoutms)
- {
- timeout = DateTime.Now;
- while (DateTime.Now < timeout.AddMilliseconds(timeoutms))
- {
- //if (item)
- {
-
- }
- }
-
- return false;
- }
-
- public bool sendRC(int channel, ushort pwm)
+
+ public bool SendRC(int channel, ushort pwm,bool sendnow)
{
switch (channel)
{
@@ -118,18 +149,18 @@ namespace ArdupilotMega
break;
}
- MainV2.comPort.sendPacket(rc);
- System.Threading.Thread.Sleep(20);
- MainV2.comPort.sendPacket(rc);
- MainV2.comPort.sendPacket(rc);
+ rc.target_component = MainV2.comPort.compid;
+ rc.target_system = MainV2.comPort.sysid;
+
+ if (sendnow)
+ {
+ MainV2.comPort.sendPacket(rc);
+ System.Threading.Thread.Sleep(20);
+ MainV2.comPort.sendPacket(rc);
+ MainV2.comPort.sendPacket(rc);
+ }
return true;
}
-
- void convertItemtoMessage()
- {
-
- }
-
}
}
diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application
index 862465c9a5..ab815306fe 100644
--- a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application
+++ b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application
@@ -11,7 +11,7 @@
- XqmS8DEyaXOEHAzbfxq+pbxDUg4=
+ QOAVY4eRCMREZxVv8+wq0bmXS7U=
diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml b/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml
index 6bb291dd77..dc4cc836fe 100644
--- a/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml
+++ b/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml
@@ -42,17 +42,19 @@
Nav Throttle
Angle boost
Manual boost
+ climb rate
+
rc3 servo out
alt hold int
thro int
- control mode
- yaw mode
- r p mode
- thro mode
- thro cruise
- thro int
+ time
+ gyro sat
+ adc const
+ renorm sqrt
+ renorm blowup
+ fix count
Gyro X
diff --git a/Tools/ArdupilotMegaPlanner/dataflashlog.xml b/Tools/ArdupilotMegaPlanner/dataflashlog.xml
index 6bb291dd77..dc4cc836fe 100644
--- a/Tools/ArdupilotMegaPlanner/dataflashlog.xml
+++ b/Tools/ArdupilotMegaPlanner/dataflashlog.xml
@@ -42,17 +42,19 @@
Nav Throttle
Angle boost
Manual boost
+ climb rate
+
rc3 servo out
alt hold int
thro int
- control mode
- yaw mode
- r p mode
- thro mode
- thro cruise
- thro int
+ time
+ gyro sat
+ adc const
+ renorm sqrt
+ renorm blowup
+ fix count
Gyro X
diff --git a/Tools/ArdupilotMegaPlanner/srtm.cs b/Tools/ArdupilotMegaPlanner/srtm.cs
index 1bc8ca401f..08e5bca0ef 100644
--- a/Tools/ArdupilotMegaPlanner/srtm.cs
+++ b/Tools/ArdupilotMegaPlanner/srtm.cs
@@ -3,6 +3,10 @@ using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
+using System.Net;
+using System.Text.RegularExpressions;
+using ICSharpCode.SharpZipLib.Zip;
+using System.Threading;
namespace ArdupilotMega
{
@@ -10,7 +14,15 @@ namespace ArdupilotMega
{
public static string datadirectory;
- public static int getAltitude(double lat, double lng)
+ static List allhgts = new List();
+
+ static object objlock = new object();
+
+ static Thread requestThread;
+
+ static List queue = new List();
+
+ public static int getAltitude(double lat, double lng, double zoom)
{
short alt = 0;
@@ -36,136 +48,296 @@ namespace ArdupilotMega
string filename2 = "srtm_" + Math.Round((lng + 2.5 + 180) / 5, 0).ToString("00") + "_" + Math.Round((60 - lat + 2.5) / 5, 0).ToString("00") + ".asc";
- if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename))
- { // srtm hgt files
- FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open, FileAccess.Read);
-
- float posx = 0;
- float row = 0;
-
- if (fs.Length <= (1201 * 1201 * 2))
- {
- posx = (int)(((float)(lng - x)) * (1201 * 2));
- row = (int)(((float)(lat - y)) * 1201) * (1201 * 2);
- row = (1201 * 1201 * 2) - row;
- }
- else
- {
- posx = (int)(((float)(lng - x)) * (3601 * 2));
- row = (int)(((float)(lat - y)) * 3601) * (3601 * 2);
- row = (3601 * 3601 * 2) - row;
- }
-
- if (posx % 2 == 1)
- {
- posx--;
- }
-
- //Console.WriteLine(filename + " row " + row + " posx" + posx);
-
- byte[] data = new byte[2];
-
- fs.Seek((int)(row + posx), SeekOrigin.Begin);
- fs.Read(data, 0, data.Length);
-
- fs.Close();
- fs.Dispose();
-
- Array.Reverse(data);
-
- alt = BitConverter.ToInt16(data, 0);
-
- return alt;
- }
- else if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename2))
+ try
{
- // this is way to slow - and cacheing it will chew memory 6001 * 6001 * 4 = 144048004 bytes
- FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename2, FileMode.Open, FileAccess.Read);
- StreamReader sr = new StreamReader(fs);
+ if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename))
+ { // srtm hgt files
+ FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open, FileAccess.Read, FileShare.Read);
- int nox = 0;
- int noy = 0;
- float left = 0;
- float top = 0;
- int nodata = -9999;
- float cellsize = 0;
+ float posx = 0;
+ float row = 0;
- int rowcounter = 0;
-
- float wantrow = 0;
- float wantcol = 0;
-
-
- while (!sr.EndOfStream)
- {
- string line = sr.ReadLine();
-
- if (line.StartsWith("ncols"))
+ if (fs.Length <= (1201 * 1201 * 2))
{
- nox = int.Parse(line.Substring(line.IndexOf(' ')));
-
- //hgtdata = new int[nox * noy];
- }
- else if (line.StartsWith("nrows"))
- {
- noy = int.Parse(line.Substring(line.IndexOf(' ')));
-
- //hgtdata = new int[nox * noy];
- }
- else if (line.StartsWith("xllcorner"))
- {
- left = float.Parse(line.Substring(line.IndexOf(' ')));
- }
- else if (line.StartsWith("yllcorner"))
- {
- top = float.Parse(line.Substring(line.IndexOf(' ')));
- }
- else if (line.StartsWith("cellsize"))
- {
- cellsize = float.Parse(line.Substring(line.IndexOf(' ')));
- }
- else if (line.StartsWith("NODATA_value"))
- {
- nodata = int.Parse(line.Substring(line.IndexOf(' ')));
+ posx = (int)(((float)(lng - x)) * (1201 * 2));
+ row = (int)(((float)(lat - y)) * 1201) * (1201 * 2);
+ row = (1201 * 1201 * 2) - row;
}
else
{
- string[] data = line.Split(new char[] { ' ' });
-
- if (data.Length == (nox + 1))
- {
-
-
-
- wantcol = (float)((lng - Math.Round(left,0)));
-
- wantrow = (float)((lat - Math.Round(top,0)));
-
- wantrow =(int)( wantrow / cellsize);
- wantcol =(int)( wantcol / cellsize);
-
- wantrow = noy - wantrow;
-
- if (rowcounter == wantrow)
- {
- Console.WriteLine("{0} {1} {2} {3} ans {4} x {5}", lng, lat, left, top, data[(int)wantcol], (nox + wantcol * cellsize));
-
- return int.Parse(data[(int)wantcol]);
- }
-
- rowcounter++;
- }
+ posx = (int)(((float)(lng - x)) * (3601 * 2));
+ row = (int)(((float)(lat - y)) * 3601) * (3601 * 2);
+ row = (3601 * 3601 * 2) - row;
}
+ if (posx % 2 == 1)
+ {
+ posx--;
+ }
+
+ //Console.WriteLine(filename + " row " + row + " posx" + posx);
+
+ byte[] data = new byte[2];
+
+ fs.Seek((int)(row + posx), SeekOrigin.Begin);
+ fs.Read(data, 0, data.Length);
+
+ fs.Close();
+ fs.Dispose();
+
+ Array.Reverse(data);
+
+ alt = BitConverter.ToInt16(data, 0);
+
+ return alt;
+ }
+ else if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename2))
+ {
+ // this is way to slow - and cacheing it will chew memory 6001 * 6001 * 4 = 144048004 bytes
+ FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename2, FileMode.Open, FileAccess.Read);
+
+ StreamReader sr = new StreamReader(fs);
+
+ int nox = 0;
+ int noy = 0;
+ float left = 0;
+ float top = 0;
+ int nodata = -9999;
+ float cellsize = 0;
+
+ int rowcounter = 0;
+
+ float wantrow = 0;
+ float wantcol = 0;
+ while (!sr.EndOfStream)
+ {
+ string line = sr.ReadLine();
+
+ if (line.StartsWith("ncols"))
+ {
+ nox = int.Parse(line.Substring(line.IndexOf(' ')));
+
+ //hgtdata = new int[nox * noy];
+ }
+ else if (line.StartsWith("nrows"))
+ {
+ noy = int.Parse(line.Substring(line.IndexOf(' ')));
+
+ //hgtdata = new int[nox * noy];
+ }
+ else if (line.StartsWith("xllcorner"))
+ {
+ left = float.Parse(line.Substring(line.IndexOf(' ')));
+ }
+ else if (line.StartsWith("yllcorner"))
+ {
+ top = float.Parse(line.Substring(line.IndexOf(' ')));
+ }
+ else if (line.StartsWith("cellsize"))
+ {
+ cellsize = float.Parse(line.Substring(line.IndexOf(' ')));
+ }
+ else if (line.StartsWith("NODATA_value"))
+ {
+ nodata = int.Parse(line.Substring(line.IndexOf(' ')));
+ }
+ else
+ {
+ string[] data = line.Split(new char[] { ' ' });
+
+ if (data.Length == (nox + 1))
+ {
+
+
+
+ wantcol = (float)((lng - Math.Round(left, 0)));
+
+ wantrow = (float)((lat - Math.Round(top, 0)));
+
+ wantrow = (int)(wantrow / cellsize);
+ wantcol = (int)(wantcol / cellsize);
+
+ wantrow = noy - wantrow;
+
+ if (rowcounter == wantrow)
+ {
+ Console.WriteLine("{0} {1} {2} {3} ans {4} x {5}", lng, lat, left, top, data[(int)wantcol], (nox + wantcol * cellsize));
+
+ return int.Parse(data[(int)wantcol]);
+ }
+
+ rowcounter++;
+ }
+ }
+
+
+
+ }
+
+ return alt;
+ }
+ else // get something
+ {
+ if (zoom >= 15)
+ {
+ if (requestThread == null)
+ {
+ Console.WriteLine("Getting " + filename);
+ queue.Add(filename);
+
+ requestThread = new Thread(requestRunner);
+ requestThread.IsBackground = true;
+ requestThread.Start();
+ }
+ else
+ {
+ lock (objlock)
+ {
+ if (!queue.Contains(filename))
+ {
+ Console.WriteLine("Getting " + filename);
+ queue.Add(filename);
+ }
+ }
+ }
+ }
}
- return alt;
}
+ catch { alt = 0; }
return alt;
}
+
+ static void requestRunner()
+ {
+ while (true)
+ {
+ try
+ {
+ string item = "";
+ lock (objlock)
+ {
+ if (queue.Count > 0)
+ {
+ item = queue[0];
+ }
+ }
+
+ if (item != "")
+ {
+ get3secfile(item);
+ lock (objlock)
+ {
+ queue.Remove(item);
+ }
+ }
+ }
+ catch { }
+ Thread.Sleep(100);
+ }
+ }
+
+ static void get3secfile(object name)
+ {
+ string baseurl = "http://dds.cr.usgs.gov/srtm/version2_1/SRTM3/";
+
+ // check file doesnt already exist
+ if (File.Exists(datadirectory + Path.DirectorySeparatorChar + (string)name))
+ {
+ return;
+ }
+
+ List list = getListing(baseurl);
+
+ foreach (string item in list)
+ {
+ List hgtfiles = getListing(item);
+
+ foreach (string hgt in hgtfiles)
+ {
+ if (hgt.Contains((string)name))
+ {
+ // get file
+
+ gethgt(hgt, (string)name);
+ return;
+ }
+ }
+ }
+ }
+
+ static void gethgt(string url, string filename)
+ {
+ try
+ {
+
+ WebRequest req = HttpWebRequest.Create(url);
+
+ WebResponse res = req.GetResponse();
+
+ Stream resstream = res.GetResponseStream();
+
+ BinaryWriter bw = new BinaryWriter(File.Create(datadirectory + Path.DirectorySeparatorChar + filename + ".zip"));
+
+ byte[] buf1 = new byte[1024];
+
+ while (resstream.CanRead)
+ {
+
+ int len = resstream.Read(buf1, 0, 1024);
+ if (len == 0)
+ break;
+ bw.Write(buf1, 0, len);
+
+ }
+
+ bw.Close();
+
+ FastZip fzip = new FastZip();
+
+ fzip.ExtractZip(datadirectory + Path.DirectorySeparatorChar + filename + ".zip", datadirectory, "");
+ }
+ catch { }
+ }
+
+ static List getListing(string url)
+ {
+ List list = new List();
+
+ try
+ {
+
+ WebRequest req = HttpWebRequest.Create(url);
+
+ WebResponse res = req.GetResponse();
+
+ StreamReader resstream = new StreamReader(res.GetResponseStream());
+
+ string data = resstream.ReadToEnd();
+
+ Regex regex = new Regex("href=\"([^\"]+)\"", RegexOptions.IgnoreCase);
+ if (regex.IsMatch(data))
+ {
+ MatchCollection matchs = regex.Matches(data);
+ for (int i = 0; i < matchs.Count; i++)
+ {
+ if (matchs[i].Groups[1].Value.ToString().Contains(".."))
+ continue;
+ if (matchs[i].Groups[1].Value.ToString().Contains("http"))
+ continue;
+
+ list.Add(url.TrimEnd(new char[] { '/', '\\' }) + "/" + matchs[i].Groups[1].Value.ToString());
+
+ }
+ }
+ }
+ catch { }
+
+ return list;
+ }
}
-}
+}
\ No newline at end of file