diff --git a/Tools/ArdupilotMegaPlanner/ArduinoDetect.cs b/Tools/ArdupilotMegaPlanner/ArduinoDetect.cs
index 01a77d1430..320338126b 100644
--- a/Tools/ArdupilotMegaPlanner/ArduinoDetect.cs
+++ b/Tools/ArdupilotMegaPlanner/ArduinoDetect.cs
@@ -10,7 +10,7 @@ namespace ArdupilotMega
class ArduinoDetect
{
///
- /// detects a 1280 or a 2560 board
+ /// detects STK version 1 or 2
///
/// comportname
/// string either (1280/2560) or "" for none
@@ -62,6 +62,85 @@ namespace ArdupilotMega
System.Threading.Thread.Sleep(100);
+ a = 0;
+ while (a < 4)
+ {
+ byte[] temp = new byte[] { 0x6, 0, 0, 0, 0 };
+ temp = ArduinoDetect.genstkv2packet(serialPort, temp);
+ a++;
+ System.Threading.Thread.Sleep(50);
+
+ try
+ {
+ if (temp[0] == 6 && temp[1] == 0 && temp.Length == 2)
+ {
+ serialPort.Close();
+
+ return "2560";
+
+ }
+ }
+ catch { }
+ }
+
+ serialPort.Close();
+ Console.WriteLine("Not a 2560");
+ return "";
+ }
+
+ ///
+ /// Detects APM board version
+ ///
+ ///
+ /// (1280/2560/2560-2)
+ public static string DetectBoard(string port)
+ {
+ SerialPort serialPort = new SerialPort();
+ serialPort.PortName = port;
+
+ if (serialPort.IsOpen)
+ serialPort.Close();
+
+ serialPort.DtrEnable = true;
+ serialPort.BaudRate = 57600;
+ serialPort.Open();
+
+ System.Threading.Thread.Sleep(100);
+
+ int a = 0;
+ while (a < 20) // 20 * 50 = 1 sec
+ {
+ //Console.WriteLine("write " + DateTime.Now.Millisecond);
+ serialPort.DiscardInBuffer();
+ serialPort.Write(new byte[] { (byte)'0', (byte)' ' }, 0, 2);
+ a++;
+ System.Threading.Thread.Sleep(50);
+
+ //Console.WriteLine("btr {0}", serialPort.BytesToRead);
+ if (serialPort.BytesToRead >= 2)
+ {
+ byte b1 = (byte)serialPort.ReadByte();
+ byte b2 = (byte)serialPort.ReadByte();
+ if (b1 == 0x14 && b2 == 0x10)
+ {
+ serialPort.Close();
+ return "1280";
+ }
+ }
+ }
+
+ serialPort.Close();
+
+ Console.WriteLine("Not a 1280");
+
+ System.Threading.Thread.Sleep(500);
+
+ serialPort.DtrEnable = true;
+ serialPort.BaudRate = 115200;
+ serialPort.Open();
+
+ System.Threading.Thread.Sleep(100);
+
a = 0;
while (a < 4)
{
@@ -101,7 +180,7 @@ namespace ArdupilotMega
}
else
{
- if (DialogResult.Yes == MessageBox.Show("APM 2.0", "Is this a APM 2?", MessageBoxButtons.YesNo))
+ if (DialogResult.Yes == MessageBox.Show("Is this a APM 2?", "APM 2", MessageBoxButtons.YesNo))
{
return "2560-2";
}
@@ -110,7 +189,7 @@ namespace ArdupilotMega
return "2560";
}
}
-
+
}
}
catch { }
@@ -119,7 +198,8 @@ namespace ArdupilotMega
serialPort.Close();
Console.WriteLine("Not a 2560");
return "";
- }
+ }
+
public static int decodeApVar(string comport, string version)
{
@@ -170,10 +250,10 @@ namespace ArdupilotMega
if (key == 0)
{
//Array.Reverse(buffer, pos, 2);
- return BitConverter.ToUInt16(buffer,pos);
+ return BitConverter.ToUInt16(buffer, pos);
}
-
+
for (int i = 0; i <= size; i++)
{
Console.Write(" {0:X2}", buffer[pos]);
@@ -283,4 +363,4 @@ namespace ArdupilotMega
return mes;
}
}
-}
+}
\ No newline at end of file
diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
index b0f33aff2c..693b390139 100644
--- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
+++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
@@ -440,9 +440,11 @@
RAW_Sensor.cs
+ Designer
MavlinkLog.cs
+ Designer
Configuration.cs
@@ -490,6 +492,7 @@
JoystickSetup.cs
+ Designer
JoystickSetup.cs
@@ -514,9 +517,11 @@
Log.cs
+ Designer
LogBrowse.cs
+ Designer
PublicResXFileCodeGenerator
@@ -533,6 +538,7 @@
Setup.cs
+ Designer
Setup.cs
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs
index 06e13f9c70..c8c07bb1c3 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs
@@ -266,7 +266,7 @@ namespace ArdupilotMega.GCSViews
fd.ShowDialog();
if (File.Exists(fd.FileName))
{
- UploadFlash(fd.FileName, ArduinoDetect.DetectVersion(MainV2.comportname));
+ UploadFlash(fd.FileName, ArduinoDetect.DetectBoard(MainV2.comportname));
}
return true;
}
@@ -498,7 +498,7 @@ namespace ArdupilotMega.GCSViews
this.Refresh();
- board = ArduinoDetect.DetectVersion(MainV2.comportname);
+ board = ArduinoDetect.DetectBoard(MainV2.comportname);
if (board == "")
{
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx
index ce57897e56..3e8d6b5151 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx
@@ -178,7 +178,7 @@
NoControl
- 563, -10
+ 399, -9
190, 190
@@ -205,7 +205,7 @@
NoControl
- 770, -10
+ 595, -8
190, 190
@@ -232,7 +232,7 @@
NoControl
- 563, 184
+ 399, 185
190, 190
@@ -259,7 +259,7 @@
NoControl
- 770, 184
+ 595, 186
190, 190
@@ -430,7 +430,7 @@
NoControl
- 560, 168
+ 396, 169
190, 13
@@ -460,7 +460,7 @@
NoControl
- 767, 168
+ 592, 170
190, 13
@@ -490,7 +490,7 @@
NoControl
- 560, 361
+ 396, 362
190, 13
@@ -520,7 +520,7 @@
NoControl
- 767, 361
+ 592, 363
190, 13
@@ -550,7 +550,7 @@
NoControl
- 310, 167
+ 200, 167
190, 13
@@ -580,7 +580,7 @@
NoControl
- 313, -9
+ 203, -9
190, 190
@@ -607,7 +607,7 @@
NoControl
- 310, 361
+ 200, 361
190, 13
@@ -637,7 +637,7 @@
NoControl
- 313, 184
+ 203, 184
190, 190
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
index 79e20164bd..3d60418cfd 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
@@ -1359,19 +1359,8 @@ namespace ArdupilotMega.GCSViews
throttle_out = ((float)MainV2.cs.hilch3 + 5000) / throttlegain;
rudder_out = (float)MainV2.cs.hilch4 / ruddergain;
}
- /*
- if ((roll_out == -1 || roll_out == 1) && (pitch_out == -1 || pitch_out == 1))
- {
- this.Invoke((MethodInvoker)delegate
- {
- try
- {
- OutputLog.AppendText("Please check your radio setup - CLI -> setup -> radio!!!\n");
- }
- catch { }
- });
- }
- */
+
+
// Limit min and max
roll_out = Constrain(roll_out, -1, 1);
pitch_out = Constrain(pitch_out, -1, 1);
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs
index 7051ee6006..241f022370 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs
@@ -155,8 +155,6 @@ namespace ArdupilotMega.GCSViews
private void Terminal_Load(object sender, EventArgs e)
{
- MessageBox.Show("Set your APM into LOG/SETUP mode!! (switch towards the servo header)");
-
try
{
MainV2.givecomport = true;
@@ -175,6 +173,11 @@ namespace ArdupilotMega.GCSViews
System.Threading.Thread t11 = new System.Threading.Thread(delegate()
{
threadrun = true;
+
+ System.Threading.Thread.Sleep(2000);
+
+ comPort.Write("\n\n\n\n\n");
+
while (threadrun)
{
try
diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.resx
index 254f6c030c..4da6d80a1b 100644
--- a/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.resx
+++ b/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.resx
@@ -136,8 +136,7 @@
0
- NOTE: You must disconnect and move the slider switch when done to use other tabs
-
+
TXT_terminal
@@ -167,7 +166,7 @@
BUTsetupshow
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.4199.27022, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
$this
@@ -191,7 +190,7 @@
BUTradiosetup
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.4199.27022, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
$this
@@ -215,7 +214,7 @@
BUTtests
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.4199.27022, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
$this
@@ -239,7 +238,7 @@
Logs
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.4199.27022, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
$this
@@ -263,7 +262,7 @@
BUT_logbrowse
- ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.4199.27022, Culture=neutral, PublicKeyToken=ff91852278f5092c
+ ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc
$this
@@ -284,6 +283,6 @@
Terminal
- System.Windows.Forms.UserControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089
+ 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/Log.cs b/Tools/ArdupilotMegaPlanner/Log.cs
index b45163c919..68e31dd776 100644
--- a/Tools/ArdupilotMegaPlanner/Log.cs
+++ b/Tools/ArdupilotMegaPlanner/Log.cs
@@ -87,6 +87,10 @@ namespace ArdupilotMega
threadrun = true;
+ System.Threading.Thread.Sleep(2000);
+
+ comPort.Write("\n\n\n\n");
+
while (threadrun)
{
try
@@ -175,7 +179,7 @@ namespace ArdupilotMega
{
case serialstatus.Connecting:
- if (line.Contains("reset to FLY") || line.Contains("interactive setup") || line.Contains("CLI:"))
+ if (line.Contains("reset to FLY") || line.Contains("interactive setup") || line.Contains("CLI:") || line.Contains("Ardu"))
{
comPort.Write("logs\r");
}
diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs
index dff779f139..4391fbd290 100644
--- a/Tools/ArdupilotMegaPlanner/MainV2.cs
+++ b/Tools/ArdupilotMegaPlanner/MainV2.cs
@@ -686,11 +686,6 @@ namespace ArdupilotMega
if (config["CHK_resetapmonconnect"] == null || bool.Parse(config["CHK_resetapmonconnect"].ToString()) == true)
comPort.BaseStream.DtrEnable = true;
- if (DialogResult.OK != Common.MessageShowAgain("Mavlink Connect", "Make sure your APM slider switch is in Flight Mode (away from RC pins)"))
- {
- return;
- }
-
try
{
if (comPort.logfile != null)
@@ -771,7 +766,7 @@ namespace ArdupilotMega
}
}
catch { }
- MessageBox.Show("Is your CLI switch in Flight position?\n(this is required for MAVlink comms)\n\n" + ex.ToString());
+ MessageBox.Show("Can not establish a connection\n\n" + ex.ToString());
return;
}
}
diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
index 76e6c3166b..bee2fdc106 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.6")]
+[assembly: AssemblyFileVersion("1.1.7")]
[assembly: NeutralResourcesLanguageAttribute("")]
diff --git a/Tools/ArdupilotMegaPlanner/RAW_Sensor.cs b/Tools/ArdupilotMegaPlanner/RAW_Sensor.cs
index e1d0f2411e..2ca1c4ee9c 100644
--- a/Tools/ArdupilotMegaPlanner/RAW_Sensor.cs
+++ b/Tools/ArdupilotMegaPlanner/RAW_Sensor.cs
@@ -291,7 +291,7 @@ namespace ArdupilotMega
}
catch
{
- MessageBox.Show("Comport open failed - Please try again and make sure your not in CLI mode");
+ MessageBox.Show("Comport open failed");
return;
}
timer1.Start();
diff --git a/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt b/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt
index 9a19416a3b..7cf9ad6228 100644
--- a/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt
+++ b/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt
@@ -116,7 +116,7 @@ It includes both fixed wing (APM) and rotary wing (!ArduCopter) parameters. Some
||LIM_PITCH_MIN||-6000||0||-2500||100||1||PITCH_MIN_DEGREE - The maximum commanded pitch down angle. Note that this value must be negative. The default is -25 degrees. Care should be taken not to set this value too large as it may result in overspeeding the aircraft.||
||GND_ALT_CM||0||500000||0||100||1||GND_ALT_CM||
||GND_ABS_PRESS|| || ||0|| || ||GND_ABS_PRESS||
-||COMPASS_DEC||-1.57075||1.57075||0||1|| ||COMPASS_DEC - Compass Declination||
+||COMPASS_DEC||-1.57075||1.57075||0||1|| ||COMPASS_DEC - Compass Declination in RADIANS||
||SR0_EXT_STAT||0||50||3||1||1||TELEMETRY_ENABLE Port 0 - Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS||
||SR0_EXTRA1||0||50||10||1||1||TELEMETRY_ENABLE Port 0 - Enable MSG_ATTITUDE||
||SR0_EXTRA2||0||50||3||1||1||TELEMETRY_ENABLE Port 0 - Enable MSG_VFR_HUD||
diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs
index 1019e1b9d9..91d69bc42e 100644
--- a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs
+++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs
@@ -377,6 +377,7 @@ namespace ArdupilotMega.Setup
if (tabControl1.SelectedTab == tabHardware)
{
startup = true;
+
if (MainV2.comPort.param["ARSPD_ENABLE"] != null)
CHK_enableairspeed.Checked = MainV2.comPort.param["ARSPD_ENABLE"].ToString() == "1" ? true : false;
@@ -391,6 +392,9 @@ namespace ArdupilotMega.Setup
if (MainV2.comPort.param["SONAR_TYPE"] != null)
CMB_sonartype.SelectedIndex = int.Parse(MainV2.comPort.param["SONAR_TYPE"].ToString());
+
+ if (MainV2.comPort.param["FLOW_ENABLE"] != null)
+ CHK_enableoptflow.Checked = MainV2.comPort.param["FLOW_ENABLE"].ToString() == "1" ? true : false;
startup = false;