Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM Planner 1.0.90 

Camera screen error fix
mavlink 1.0 now compile time option
Configuration screen modify
dataflashlog update
This commit is contained in:
Michael Oborne 2011-11-08 21:22:07 +08:00
parent 3427adbebe
commit 9aff265e77
25 changed files with 3628 additions and 1283 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
View File

@ -42,7 +42,7 @@
<DebugType>full</DebugType>
<Optimize>false</Optimize>
<OutputPath>bin\Debug\</OutputPath>
<DefineConstants>TRACE;DEBUG</DefineConstants>
<DefineConstants>TRACE;DEBUG;MAVLINK10cra</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
<AllowUnsafeBlocks>false</AllowUnsafeBlocks>
@ -57,7 +57,7 @@
<DebugType>full</DebugType>
<Optimize>true</Optimize>
<OutputPath>bin\Release\</OutputPath>
<DefineConstants>TRACE</DefineConstants>
<DefineConstants>TRACE;MAVLINK10cra</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
<AllowUnsafeBlocks>false</AllowUnsafeBlocks>
@ -227,6 +227,7 @@
<Compile Include="JoystickSetup.Designer.cs">
<DependentUpon>JoystickSetup.cs</DependentUpon>
</Compile>
<Compile Include="MAVLinkTypes.cs" />
<Compile Include="MAVLinkTypesenum.cs" />
<Compile Include="MyButton.cs">
<SubType>Component</SubType>
@ -302,7 +303,7 @@
<SubType>Component</SubType>
</Compile>
<Compile Include="CurrentState.cs" />
<Compile Include="MAVLinkTypes.cs" />
<Compile Include="MAVLinkTypes0.9.cs" />
<Compile Include="ElevationProfile.cs">
<SubType>Form</SubType>
</Compile>

3
Tools/ArdupilotMegaPlanner/ArdupilotMega.sln
View File

@ -2,9 +2,6 @@
Microsoft Visual Studio Solution File, Format Version 11.00
# Visual C# Express 2010
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ArdupilotMega", "ArdupilotMega.csproj", "{A2E22272-95FE-47B6-B050-9AE7E2055BF5}"
ProjectSection(ProjectDependencies) = postProject
{E64A1A41-A5B0-458E-8284-BB63705354DA} = {E64A1A41-A5B0-458E-8284-BB63705354DA}
EndProjectSection
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Updater", "Updater\Updater.csproj", "{E64A1A41-A5B0-458E-8284-BB63705354DA}"
EndProject

37
Tools/ArdupilotMegaPlanner/AviWriter.cs
View File

@ -195,7 +195,7 @@ SizeConst = 4)]
{
Console.WriteLine(DateTime.Now.Millisecond + "avi frame");
db_head db = new db_head { db = "00dc".ToCharArray(), size = size };
fd.Write(ArdupilotMega.MAVLink.StructureToByteArray(db), 0, Marshal.SizeOf(db));
fd.Write(StructureToByteArray(db), 0, Marshal.SizeOf(db));
fd.Write(buf, 0, (int)size);
if (size % 2 == 1)
{
@ -276,16 +276,35 @@ SizeConst = 4)]
long pos = fd.Position;
fd.Seek(0, SeekOrigin.Begin);
fd.Write(ArdupilotMega.MAVLink.StructureToByteArray(rh),0, Marshal.SizeOf(rh));
fd.Write(ArdupilotMega.MAVLink.StructureToByteArray(lh1), 0, Marshal.SizeOf(lh1));
fd.Write(ArdupilotMega.MAVLink.StructureToByteArray(ah), 0, Marshal.SizeOf(ah));
fd.Write(ArdupilotMega.MAVLink.StructureToByteArray(lh2), 0, Marshal.SizeOf(lh2));
fd.Write(ArdupilotMega.MAVLink.StructureToByteArray(sh), 0, Marshal.SizeOf(sh));
fd.Write(ArdupilotMega.MAVLink.StructureToByteArray(fh), 0, Marshal.SizeOf(fh));
fd.Write(ArdupilotMega.MAVLink.StructureToByteArray(junk), 0, Marshal.SizeOf(junk));
fd.Write(StructureToByteArray(rh),0, Marshal.SizeOf(rh));
fd.Write(StructureToByteArray(lh1), 0, Marshal.SizeOf(lh1));
fd.Write(StructureToByteArray(ah), 0, Marshal.SizeOf(ah));
fd.Write(StructureToByteArray(lh2), 0, Marshal.SizeOf(lh2));
fd.Write(StructureToByteArray(sh), 0, Marshal.SizeOf(sh));
fd.Write(StructureToByteArray(fh), 0, Marshal.SizeOf(fh));
fd.Write(StructureToByteArray(junk), 0, Marshal.SizeOf(junk));
fd.Seek(2036, SeekOrigin.Begin);
fd.Write(ArdupilotMega.MAVLink.StructureToByteArray(lh3), 0, Marshal.SizeOf(lh3));
fd.Write(StructureToByteArray(lh3), 0, Marshal.SizeOf(lh3));
fd.Seek(pos, SeekOrigin.Begin);
}
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;
}
}

75
Tools/ArdupilotMegaPlanner/Camera.cs
View File

@ -36,43 +36,48 @@ namespace ArdupilotMega
void doCalc()
{
// entered values
float focallen = (float)num_focallength.Value;
float flyalt = (float)num_agl.Value;
int imagewidth = int.Parse(TXT_imgwidth.Text);
int imageheight = int.Parse(TXT_imgheight.Text);
float sensorwidth = float.Parse(TXT_senswidth.Text);
float sensorheight = float.Parse(TXT_sensheight.Text);
int overlap = (int)num_overlap.Value;
int sidelap = (int)num_sidelap.Value;
// scale
float flscale = 1000 * flyalt / focallen;
float viewwidth = (sensorwidth * flscale / 1000);
float viewheight = (sensorheight * flscale / 1000);
TXT_fovH.Text = (viewwidth).ToString();
TXT_fovV.Text = (viewheight).ToString();
TXT_fovAH.Text = (Math.Atan(sensorwidth / (2 * focallen)) * rad2deg * 2).ToString();
TXT_fovAV.Text = (Math.Atan(sensorheight / (2 * focallen)) * rad2deg * 2).ToString();
TXT_cmpixel.Text = ((viewheight / imageheight) * 100).ToString("0.00 cm");
if (CHK_camdirection.Checked)
try
{
TXT_distflphotos.Text = ((1 - (overlap / 100.0f)) * viewheight).ToString();
TXT_distacflphotos.Text = ((1 - (sidelap / 100.0f)) * viewwidth).ToString();
}
else
{
TXT_distflphotos.Text = ((1 - (overlap / 100.0f)) * viewwidth).ToString();
TXT_distacflphotos.Text = ((1 - (sidelap / 100.0f)) * viewheight).ToString();
// entered values
float focallen = (float)num_focallength.Value;
float flyalt = (float)num_agl.Value;
int imagewidth = int.Parse(TXT_imgwidth.Text);
int imageheight = int.Parse(TXT_imgheight.Text);
float sensorwidth = float.Parse(TXT_senswidth.Text);
float sensorheight = float.Parse(TXT_sensheight.Text);
int overlap = (int)num_overlap.Value;
int sidelap = (int)num_sidelap.Value;
// scale
float flscale = 1000 * flyalt / focallen;
float viewwidth = (sensorwidth * flscale / 1000);
float viewheight = (sensorheight * flscale / 1000);
TXT_fovH.Text = (viewwidth).ToString();
TXT_fovV.Text = (viewheight).ToString();
TXT_fovAH.Text = (Math.Atan(sensorwidth / (2 * focallen)) * rad2deg * 2).ToString();
TXT_fovAV.Text = (Math.Atan(sensorheight / (2 * focallen)) * rad2deg * 2).ToString();
TXT_cmpixel.Text = ((viewheight / imageheight) * 100).ToString("0.00 cm");
if (CHK_camdirection.Checked)
{
TXT_distflphotos.Text = ((1 - (overlap / 100.0f)) * viewheight).ToString();
TXT_distacflphotos.Text = ((1 - (sidelap / 100.0f)) * viewwidth).ToString();
}
else
{
TXT_distflphotos.Text = ((1 - (overlap / 100.0f)) * viewwidth).ToString();
TXT_distacflphotos.Text = ((1 - (sidelap / 100.0f)) * viewheight).ToString();
}
}
catch { return; }
}
private void num_agl_ValueChanged(object sender, EventArgs e)

107
Tools/ArdupilotMegaPlanner/Common.cs
View File

@ -249,6 +249,62 @@ namespace ArdupilotMega
POSITION = 8
}
#if MAVLINK10
public static bool translateMode(string modein, ref MAVLink.__mavlink_set_mode_t mode)
{
//MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t();
mode.target_system = MainV2.comPort.sysid;
try
{
if (Common.getModes() == typeof(Common.apmmodes))
{
switch ((int)Enum.Parse(Common.getModes(), modein))
{
case (int)Common.apmmodes.MANUAL:
case (int)Common.apmmodes.CIRCLE:
case (int)Common.apmmodes.STABILIZE:
case (int)Common.apmmodes.AUTO:
case (int)Common.apmmodes.RTL:
case (int)Common.apmmodes.LOITER:
case (int)Common.apmmodes.FLY_BY_WIRE_A:
case (int)Common.apmmodes.FLY_BY_WIRE_B:
mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
mode.custom_mode = (uint)(int)Enum.Parse(Common.getModes(), modein);
break;
default:
MessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein));
return false;
}
}
else if (Common.getModes() == typeof(Common.ac2modes))
{
switch ((int)Enum.Parse(Common.getModes(), modein))
{
case (int)Common.ac2modes.STABILIZE:
case (int)Common.ac2modes.AUTO:
case (int)Common.ac2modes.RTL:
case (int)Common.ac2modes.LOITER:
case (int)Common.ac2modes.ACRO:
case (int)Common.ac2modes.ALT_HOLD:
case (int)Common.ac2modes.CIRCLE:
case (int)Common.ac2modes.POSITION:
mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
mode.custom_mode = (uint)(int)Enum.Parse(Common.getModes(), modein);
break;
default:
MessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein));
return false;
}
}
}
catch { System.Windows.Forms.MessageBox.Show("Failed to find Mode"); return false; }
return true;
}
#else
public static bool translateMode(string modein, ref MAVLink.__mavlink_set_nav_mode_t navmode, ref MAVLink.__mavlink_set_mode_t mode)
{
@ -334,6 +390,7 @@ namespace ArdupilotMega
return true;
}
#endif
public static bool getFilefromNet(string url,string saveto) {
try
@ -592,6 +649,8 @@ namespace ArdupilotMega
int _value = 0;
System.Windows.Forms.Label lbl1 = new System.Windows.Forms.Label();
System.Windows.Forms.Label lbl = new System.Windows.Forms.Label();
public bool reverse = false;
int displayvalue = 0;
public HorizontalProgressBar2()
: base()
@ -606,7 +665,15 @@ namespace ArdupilotMega
if (_value == value)
return;
_value = value;
int ans = value + offset;
displayvalue = _value;
if (reverse)
{
int dif = _value - Minimum;
_value = Maximum - dif;
}
int ans = _value + offset;
if (ans <= base.Minimum)
{
ans = base.Minimum + 1; // prevent an exception for the -1 hack
@ -675,7 +742,7 @@ System.ComponentModel.Description("Text under Bar")]
lbl1.Location = new Point(this.Location.X, this.Location.Y + this.Height + 15);
lbl1.ClientSize = new Size(this.Width, 13);
lbl1.TextAlign = ContentAlignment.MiddleCenter;
lbl1.Text = Value.ToString();
lbl1.Text = displayvalue.ToString();
if (minline != 0 && maxline != 0)
{
@ -688,15 +755,35 @@ System.ComponentModel.Description("Text under Bar")]
if ((Type)this.GetType() == typeof(VerticalProgressBar2)) {
e.ResetTransform();
range2 = this.Height;
e.DrawLine(redPen, 0, (this.Maximum - minline) / range * range2 + 0, this.Width, (this.Maximum - minline) / range * range2 + 0);
e.DrawLine(redPen, 0, (this.Maximum - maxline) / range * range2 + 6, this.Width, (this.Maximum - maxline) / range * range2 + 6);
e.DrawString(minline.ToString(), SystemFonts.DefaultFont, mybrush, 5, (this.Maximum - minline) / range * range2 + 2);
e.DrawString(maxline.ToString(), SystemFonts.DefaultFont, Brushes.White, 5, (this.Maximum - maxline) / range * range2 - 10);
if (reverse)
{
e.DrawLine(redPen, 0, (maxline - this.Minimum) / range * range2 + 0, this.Width, (maxline - this.Minimum) / range * range2 + 0);
e.DrawLine(redPen, 0, (minline - this.Minimum) / range * range2 + 6, this.Width, (minline - this.Minimum) / range * range2 + 6);
e.DrawString(maxline.ToString(), SystemFonts.DefaultFont, mybrush, 5, (maxline - this.Minimum) / range * range2 + 2);
e.DrawString(minline.ToString(), SystemFonts.DefaultFont, Brushes.White, 5, (minline - this.Minimum) / range * range2 - 10);
}
else
{
e.DrawLine(redPen, 0, (this.Maximum - minline) / range * range2 + 0, this.Width, (this.Maximum - minline) / range * range2 + 0);
e.DrawLine(redPen, 0, (this.Maximum - maxline) / range * range2 + 6, this.Width, (this.Maximum - maxline) / range * range2 + 6);
e.DrawString(minline.ToString(), SystemFonts.DefaultFont, mybrush, 5, (this.Maximum - minline) / range * range2 + 2);
e.DrawString(maxline.ToString(), SystemFonts.DefaultFont, Brushes.White, 5, (this.Maximum - maxline) / range * range2 - 10);
}
} else {
e.DrawLine(redPen, (minline - this.Minimum) / range * range2 - 0, 0, (minline - this.Minimum) / range * range2 - 0, this.Height);
e.DrawLine(redPen, (maxline - this.Minimum) / range * range2 - 0, 0, (maxline - this.Minimum) / range * range2 - 0, this.Height);
e.DrawString(minline.ToString(), SystemFonts.DefaultFont, mybrush, (minline - this.Minimum) / range * range2 - 30, 5);
e.DrawString(maxline.ToString(), SystemFonts.DefaultFont, Brushes.White, (maxline - this.Minimum) / range * range2 - 0, 5);
if (reverse)
{
e.DrawLine(redPen, (this.Maximum - minline) / range * range2 - 0, 0, (this.Maximum - minline) / range * range2 - 0, this.Height);
e.DrawLine(redPen, (this.Maximum - maxline) / range * range2 - 0, 0, (this.Maximum - maxline) / range * range2 - 0, this.Height);
e.DrawString(minline.ToString(), SystemFonts.DefaultFont, mybrush, (this.Maximum - minline) / range * range2 - 30, 5);
e.DrawString(maxline.ToString(), SystemFonts.DefaultFont, Brushes.White, (this.Maximum - maxline) / range * range2 - 0, 5);
}
else
{
e.DrawLine(redPen, (minline - this.Minimum) / range * range2 - 0, 0, (minline - this.Minimum) / range * range2 - 0, this.Height);
e.DrawLine(redPen, (maxline - this.Minimum) / range * range2 - 0, 0, (maxline - this.Minimum) / range * range2 - 0, this.Height);
e.DrawString(minline.ToString(), SystemFonts.DefaultFont, mybrush, (minline - this.Minimum) / range * range2 - 30, 5);
e.DrawString(maxline.ToString(), SystemFonts.DefaultFont, Brushes.White, (maxline - this.Minimum) / range * range2 - 0, 5);
}
}
}
}

172
Tools/ArdupilotMegaPlanner/CurrentState.cs
View File

@ -272,6 +272,119 @@ namespace ArdupilotMega
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
}
#if MAVLINK10
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null)
{
ArdupilotMega.MAVLink.__mavlink_heartbeat_t hb = new ArdupilotMega.MAVLink.__mavlink_heartbeat_t();
object temp = hb;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT], ref temp, 6);
hb = (ArdupilotMega.MAVLink.__mavlink_heartbeat_t)(temp);
string oldmode = mode;
mode = "Unknown";
if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) != 0)
{
if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_FIXED_WING)
{
switch (hb.custom_mode)
{
case (byte)(Common.apmmodes.MANUAL):
mode = "Manual";
break;
case (byte)(Common.apmmodes.GUIDED):
mode = "Guided";
break;
case (byte)(Common.apmmodes.STABILIZE):
mode = "Stabilize";
break;
case (byte)(Common.apmmodes.FLY_BY_WIRE_A):
mode = "FBW A";
break;
case (byte)(Common.apmmodes.FLY_BY_WIRE_B):
mode = "FBW B";
break;
case (byte)(Common.apmmodes.AUTO):
mode = "Auto";
break;
case (byte)(Common.apmmodes.RTL):
mode = "RTL";
break;
case (byte)(Common.apmmodes.LOITER):
mode = "Loiter";
break;
case (byte)(Common.apmmodes.CIRCLE):
mode = "Circle";
break;
default:
mode = "Unknown";
break;
}
}
else if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_QUADROTOR)
{
switch (hb.custom_mode)
{
case (byte)(Common.ac2modes.STABILIZE):
mode = "Stabilize";
break;
case (byte)(Common.ac2modes.ACRO):
mode = "Acro";
break;
case (byte)(Common.ac2modes.ALT_HOLD):
mode = "Alt Hold";
break;
case (byte)(Common.ac2modes.AUTO):
mode = "Auto";
break;
case (byte)(Common.ac2modes.GUIDED):
mode = "Guided";
break;
case (byte)(Common.ac2modes.LOITER):
mode = "Loiter";
break;
case (byte)(Common.ac2modes.RTL):
mode = "RTL";
break;
case (byte)(Common.ac2modes.CIRCLE):
mode = "Circle";
break;
default:
mode = "Unknown";
break;
}
}
}
if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
}
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
{
ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t();
object temp = sysstatus;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6);
sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp);
battery_voltage = sysstatus.voltage_battery;
battery_remaining = sysstatus.battery_remaining;
packetdropremote = sysstatus.drop_rate_comm;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
#else
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
{
@ -385,7 +498,7 @@ namespace ArdupilotMega
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
#endif
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] != null)
{
var att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();
@ -404,6 +517,32 @@ namespace ArdupilotMega
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null;
}
#if MAVLINK10
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null)
{
var gps = new MAVLink.__mavlink_gps_raw_int_t();
object temp = gps;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], ref temp, 6);
gps = (MAVLink.__mavlink_gps_raw_int_t)(temp);
lat = gps.lat * 1.0e-7f;
lng = gps.lon * 1.0e-7f;
// alt = gps.alt; // using vfr as includes baro calc
gpsstatus = gps.fix_type;
// Console.WriteLine("gpsfix {0}",gpsstatus);
gpshdop = gps.eph;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
#else
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] != null)
{
@ -429,7 +568,7 @@ namespace ArdupilotMega
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
#endif
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS] != null)
{
var gps = new MAVLink.__mavlink_gps_status_t();
@ -442,7 +581,6 @@ namespace ArdupilotMega
satcount = gps.satellites_visible;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] != null)
{
var loc = new MAVLink.__mavlink_global_position_int_t();
@ -453,12 +591,35 @@ namespace ArdupilotMega
loc = (MAVLink.__mavlink_global_position_int_t)(temp);
alt = loc.alt / 1000.0f;
//alt = loc.alt / 1000.0f;
lat = loc.lat / 10000000.0f;
lng = loc.lon / 10000000.0f;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION] != null)
#if MAVLINK10
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT] != null)
{
ArdupilotMega.MAVLink.__mavlink_mission_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_mission_current_t();
object temp = wpcur;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT], ref temp, 6);
wpcur = (ArdupilotMega.MAVLink.__mavlink_mission_current_t)(temp);
int oldwp = (int)wpno;
wpno = wpcur.seq;
if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True")
{
MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
}
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
}
#else
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION] != null)
{
var loc = new MAVLink.__mavlink_global_position_t();
@ -495,6 +656,7 @@ namespace ArdupilotMega
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
}
#endif
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] != null)
{
var rcin = new MAVLink.__mavlink_rc_channels_raw_t();

340
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs generated
View File

@ -41,187 +41,187 @@
this.ConfigTabs = new System.Windows.Forms.TabControl();
this.TabAPM2 = new System.Windows.Forms.TabPage();
this.groupBox3 = new System.Windows.Forms.GroupBox();
this.THR_FS_VALUE = new System.Windows.Forms.DomainUpDown();
this.THR_FS_VALUE = new System.Windows.Forms.NumericUpDown();
this.label5 = new System.Windows.Forms.Label();
this.THR_MAX = new System.Windows.Forms.DomainUpDown();
this.THR_MAX = new System.Windows.Forms.NumericUpDown();
this.label6 = new System.Windows.Forms.Label();
this.THR_MIN = new System.Windows.Forms.DomainUpDown();
this.THR_MIN = new System.Windows.Forms.NumericUpDown();
this.label7 = new System.Windows.Forms.Label();
this.TRIM_THROTTLE = new System.Windows.Forms.DomainUpDown();
this.TRIM_THROTTLE = new System.Windows.Forms.NumericUpDown();
this.label8 = new System.Windows.Forms.Label();
this.groupBox1 = new System.Windows.Forms.GroupBox();
this.ARSPD_RATIO = new System.Windows.Forms.DomainUpDown();
this.ARSPD_RATIO = new System.Windows.Forms.NumericUpDown();
this.label1 = new System.Windows.Forms.Label();
this.ARSPD_FBW_MAX = new System.Windows.Forms.DomainUpDown();
this.ARSPD_FBW_MAX = new System.Windows.Forms.NumericUpDown();
this.label2 = new System.Windows.Forms.Label();
this.ARSPD_FBW_MIN = new System.Windows.Forms.DomainUpDown();
this.ARSPD_FBW_MIN = new System.Windows.Forms.NumericUpDown();
this.label3 = new System.Windows.Forms.Label();
this.TRIM_ARSPD_CM = new System.Windows.Forms.DomainUpDown();
this.TRIM_ARSPD_CM = new System.Windows.Forms.NumericUpDown();
this.label4 = new System.Windows.Forms.Label();
this.groupBox2 = new System.Windows.Forms.GroupBox();
this.LIM_PITCH_MIN = new System.Windows.Forms.DomainUpDown();
this.LIM_PITCH_MIN = new System.Windows.Forms.NumericUpDown();
this.label39 = new System.Windows.Forms.Label();
this.LIM_PITCH_MAX = new System.Windows.Forms.DomainUpDown();
this.LIM_PITCH_MAX = new System.Windows.Forms.NumericUpDown();
this.label38 = new System.Windows.Forms.Label();
this.LIM_ROLL_CD = new System.Windows.Forms.DomainUpDown();
this.LIM_ROLL_CD = new System.Windows.Forms.NumericUpDown();
this.label37 = new System.Windows.Forms.Label();
this.groupBox15 = new System.Windows.Forms.GroupBox();
this.XTRK_ANGLE_CD = new System.Windows.Forms.DomainUpDown();
this.XTRK_ANGLE_CD = new System.Windows.Forms.NumericUpDown();
this.label79 = new System.Windows.Forms.Label();
this.XTRK_GAIN_SC = new System.Windows.Forms.DomainUpDown();
this.XTRK_GAIN_SC = new System.Windows.Forms.NumericUpDown();
this.label80 = new System.Windows.Forms.Label();
this.groupBox16 = new System.Windows.Forms.GroupBox();
this.KFF_PTCH2THR = new System.Windows.Forms.DomainUpDown();
this.KFF_PTCH2THR = new System.Windows.Forms.NumericUpDown();
this.label83 = new System.Windows.Forms.Label();
this.KFF_RDDRMIX = new System.Windows.Forms.DomainUpDown();
this.KFF_RDDRMIX = new System.Windows.Forms.NumericUpDown();
this.label78 = new System.Windows.Forms.Label();
this.KFF_PTCHCOMP = new System.Windows.Forms.DomainUpDown();
this.KFF_PTCHCOMP = new System.Windows.Forms.NumericUpDown();
this.label81 = new System.Windows.Forms.Label();
this.groupBox14 = new System.Windows.Forms.GroupBox();
this.ENRGY2THR_IMAX = new System.Windows.Forms.DomainUpDown();
this.ENRGY2THR_IMAX = new System.Windows.Forms.NumericUpDown();
this.label73 = new System.Windows.Forms.Label();
this.ENRGY2THR_D = new System.Windows.Forms.DomainUpDown();
this.ENRGY2THR_D = new System.Windows.Forms.NumericUpDown();
this.label74 = new System.Windows.Forms.Label();
this.ENRGY2THR_I = new System.Windows.Forms.DomainUpDown();
this.ENRGY2THR_I = new System.Windows.Forms.NumericUpDown();
this.label75 = new System.Windows.Forms.Label();
this.ENRGY2THR_P = new System.Windows.Forms.DomainUpDown();
this.ENRGY2THR_P = new System.Windows.Forms.NumericUpDown();
this.label76 = new System.Windows.Forms.Label();
this.groupBox13 = new System.Windows.Forms.GroupBox();
this.ALT2PTCH_IMAX = new System.Windows.Forms.DomainUpDown();
this.ALT2PTCH_IMAX = new System.Windows.Forms.NumericUpDown();
this.label69 = new System.Windows.Forms.Label();
this.ALT2PTCH_D = new System.Windows.Forms.DomainUpDown();
this.ALT2PTCH_D = new System.Windows.Forms.NumericUpDown();
this.label70 = new System.Windows.Forms.Label();
this.ALT2PTCH_I = new System.Windows.Forms.DomainUpDown();
this.ALT2PTCH_I = new System.Windows.Forms.NumericUpDown();
this.label71 = new System.Windows.Forms.Label();
this.ALT2PTCH_P = new System.Windows.Forms.DomainUpDown();
this.ALT2PTCH_P = new System.Windows.Forms.NumericUpDown();
this.label72 = new System.Windows.Forms.Label();
this.groupBox12 = new System.Windows.Forms.GroupBox();
this.ARSP2PTCH_IMAX = new System.Windows.Forms.DomainUpDown();
this.ARSP2PTCH_IMAX = new System.Windows.Forms.NumericUpDown();
this.label65 = new System.Windows.Forms.Label();
this.ARSP2PTCH_D = new System.Windows.Forms.DomainUpDown();
this.ARSP2PTCH_D = new System.Windows.Forms.NumericUpDown();
this.label66 = new System.Windows.Forms.Label();
this.ARSP2PTCH_I = new System.Windows.Forms.DomainUpDown();
this.ARSP2PTCH_I = new System.Windows.Forms.NumericUpDown();
this.label67 = new System.Windows.Forms.Label();
this.ARSP2PTCH_P = new System.Windows.Forms.DomainUpDown();
this.ARSP2PTCH_P = new System.Windows.Forms.NumericUpDown();
this.label68 = new System.Windows.Forms.Label();
this.groupBox11 = new System.Windows.Forms.GroupBox();
this.HDNG2RLL_IMAX = new System.Windows.Forms.DomainUpDown();
this.HDNG2RLL_IMAX = new System.Windows.Forms.NumericUpDown();
this.label61 = new System.Windows.Forms.Label();
this.HDNG2RLL_D = new System.Windows.Forms.DomainUpDown();
this.HDNG2RLL_D = new System.Windows.Forms.NumericUpDown();
this.label62 = new System.Windows.Forms.Label();
this.HDNG2RLL_I = new System.Windows.Forms.DomainUpDown();
this.HDNG2RLL_I = new System.Windows.Forms.NumericUpDown();
this.label63 = new System.Windows.Forms.Label();
this.HDNG2RLL_P = new System.Windows.Forms.DomainUpDown();
this.HDNG2RLL_P = new System.Windows.Forms.NumericUpDown();
this.label64 = new System.Windows.Forms.Label();
this.groupBox10 = new System.Windows.Forms.GroupBox();
this.YW2SRV_IMAX = new System.Windows.Forms.DomainUpDown();
this.YW2SRV_IMAX = new System.Windows.Forms.NumericUpDown();
this.label57 = new System.Windows.Forms.Label();
this.YW2SRV_D = new System.Windows.Forms.DomainUpDown();
this.YW2SRV_D = new System.Windows.Forms.NumericUpDown();
this.label58 = new System.Windows.Forms.Label();
this.YW2SRV_I = new System.Windows.Forms.DomainUpDown();
this.YW2SRV_I = new System.Windows.Forms.NumericUpDown();
this.label59 = new System.Windows.Forms.Label();
this.YW2SRV_P = new System.Windows.Forms.DomainUpDown();
this.YW2SRV_P = new System.Windows.Forms.NumericUpDown();
this.label60 = new System.Windows.Forms.Label();
this.groupBox9 = new System.Windows.Forms.GroupBox();
this.PTCH2SRV_IMAX = new System.Windows.Forms.DomainUpDown();
this.PTCH2SRV_IMAX = new System.Windows.Forms.NumericUpDown();
this.label53 = new System.Windows.Forms.Label();
this.PTCH2SRV_D = new System.Windows.Forms.DomainUpDown();
this.PTCH2SRV_D = new System.Windows.Forms.NumericUpDown();
this.label54 = new System.Windows.Forms.Label();
this.PTCH2SRV_I = new System.Windows.Forms.DomainUpDown();
this.PTCH2SRV_I = new System.Windows.Forms.NumericUpDown();
this.label55 = new System.Windows.Forms.Label();
this.PTCH2SRV_P = new System.Windows.Forms.DomainUpDown();
this.PTCH2SRV_P = new System.Windows.Forms.NumericUpDown();
this.label56 = new System.Windows.Forms.Label();
this.groupBox8 = new System.Windows.Forms.GroupBox();
this.RLL2SRV_IMAX = new System.Windows.Forms.DomainUpDown();
this.RLL2SRV_IMAX = new System.Windows.Forms.NumericUpDown();
this.label49 = new System.Windows.Forms.Label();
this.RLL2SRV_D = new System.Windows.Forms.DomainUpDown();
this.RLL2SRV_D = new System.Windows.Forms.NumericUpDown();
this.label50 = new System.Windows.Forms.Label();
this.RLL2SRV_I = new System.Windows.Forms.DomainUpDown();
this.RLL2SRV_I = new System.Windows.Forms.NumericUpDown();
this.label51 = new System.Windows.Forms.Label();
this.RLL2SRV_P = new System.Windows.Forms.DomainUpDown();
this.RLL2SRV_P = new System.Windows.Forms.NumericUpDown();
this.label52 = new System.Windows.Forms.Label();
this.TabAC2 = new System.Windows.Forms.TabPage();
this.groupBox5 = new System.Windows.Forms.GroupBox();
this.label14 = new System.Windows.Forms.Label();
this.THR_RATE_IMAX = new System.Windows.Forms.DomainUpDown();
this.THR_RATE_I = new System.Windows.Forms.DomainUpDown();
this.THR_RATE_IMAX = new System.Windows.Forms.NumericUpDown();
this.THR_RATE_I = new System.Windows.Forms.NumericUpDown();
this.label20 = new System.Windows.Forms.Label();
this.THR_RATE_P = new System.Windows.Forms.DomainUpDown();
this.THR_RATE_P = new System.Windows.Forms.NumericUpDown();
this.label25 = new System.Windows.Forms.Label();
this.CHK_lockrollpitch = new System.Windows.Forms.CheckBox();
this.groupBox4 = new System.Windows.Forms.GroupBox();
this.WP_SPEED_MAX = new System.Windows.Forms.DomainUpDown();
this.WP_SPEED_MAX = new System.Windows.Forms.NumericUpDown();
this.label9 = new System.Windows.Forms.Label();
this.NAV_LAT_IMAX = new System.Windows.Forms.DomainUpDown();
this.NAV_LAT_IMAX = new System.Windows.Forms.NumericUpDown();
this.label13 = new System.Windows.Forms.Label();
this.NAV_LAT_I = new System.Windows.Forms.DomainUpDown();
this.NAV_LAT_I = new System.Windows.Forms.NumericUpDown();
this.label15 = new System.Windows.Forms.Label();
this.NAV_LAT_P = new System.Windows.Forms.DomainUpDown();
this.NAV_LAT_P = new System.Windows.Forms.NumericUpDown();
this.label16 = new System.Windows.Forms.Label();
this.groupBox6 = new System.Windows.Forms.GroupBox();
this.XTRK_ANGLE_CD1 = new System.Windows.Forms.DomainUpDown();
this.XTRK_ANGLE_CD1 = new System.Windows.Forms.NumericUpDown();
this.label10 = new System.Windows.Forms.Label();
this.XTRACK_IMAX = new System.Windows.Forms.DomainUpDown();
this.XTRACK_IMAX = new System.Windows.Forms.NumericUpDown();
this.label11 = new System.Windows.Forms.Label();
this.XTRACK_I = new System.Windows.Forms.DomainUpDown();
this.XTRACK_I = new System.Windows.Forms.NumericUpDown();
this.label17 = new System.Windows.Forms.Label();
this.XTRACK_P = new System.Windows.Forms.DomainUpDown();
this.XTRACK_P = new System.Windows.Forms.NumericUpDown();
this.label18 = new System.Windows.Forms.Label();
this.groupBox7 = new System.Windows.Forms.GroupBox();
this.THR_ALT_IMAX = new System.Windows.Forms.DomainUpDown();
this.THR_ALT_IMAX = new System.Windows.Forms.NumericUpDown();
this.label19 = new System.Windows.Forms.Label();
this.THR_ALT_I = new System.Windows.Forms.DomainUpDown();
this.THR_ALT_I = new System.Windows.Forms.NumericUpDown();
this.label21 = new System.Windows.Forms.Label();
this.THR_ALT_P = new System.Windows.Forms.DomainUpDown();
this.THR_ALT_P = new System.Windows.Forms.NumericUpDown();
this.label22 = new System.Windows.Forms.Label();
this.groupBox19 = new System.Windows.Forms.GroupBox();
this.HLD_LAT_IMAX = new System.Windows.Forms.DomainUpDown();
this.HLD_LAT_IMAX = new System.Windows.Forms.NumericUpDown();
this.label28 = new System.Windows.Forms.Label();
this.HLD_LAT_I = new System.Windows.Forms.DomainUpDown();
this.HLD_LAT_I = new System.Windows.Forms.NumericUpDown();
this.label30 = new System.Windows.Forms.Label();
this.HLD_LAT_P = new System.Windows.Forms.DomainUpDown();
this.HLD_LAT_P = new System.Windows.Forms.NumericUpDown();
this.label31 = new System.Windows.Forms.Label();
this.groupBox20 = new System.Windows.Forms.GroupBox();
this.STB_YAW_IMAX = new System.Windows.Forms.DomainUpDown();
this.STB_YAW_IMAX = new System.Windows.Forms.NumericUpDown();
this.label32 = new System.Windows.Forms.Label();
this.STB_YAW_I = new System.Windows.Forms.DomainUpDown();
this.STB_YAW_I = new System.Windows.Forms.NumericUpDown();
this.label34 = new System.Windows.Forms.Label();
this.STB_YAW_P = new System.Windows.Forms.DomainUpDown();
this.STB_YAW_P = new System.Windows.Forms.NumericUpDown();
this.label35 = new System.Windows.Forms.Label();
this.groupBox21 = new System.Windows.Forms.GroupBox();
this.STB_PIT_IMAX = new System.Windows.Forms.DomainUpDown();
this.STB_PIT_IMAX = new System.Windows.Forms.NumericUpDown();
this.label36 = new System.Windows.Forms.Label();
this.STB_PIT_I = new System.Windows.Forms.DomainUpDown();
this.STB_PIT_I = new System.Windows.Forms.NumericUpDown();
this.label41 = new System.Windows.Forms.Label();
this.STB_PIT_P = new System.Windows.Forms.DomainUpDown();
this.STB_PIT_P = new System.Windows.Forms.NumericUpDown();
this.label42 = new System.Windows.Forms.Label();
this.groupBox22 = new System.Windows.Forms.GroupBox();
this.STB_RLL_IMAX = new System.Windows.Forms.DomainUpDown();
this.STB_RLL_IMAX = new System.Windows.Forms.NumericUpDown();
this.label43 = new System.Windows.Forms.Label();
this.STB_RLL_I = new System.Windows.Forms.DomainUpDown();
this.STB_RLL_I = new System.Windows.Forms.NumericUpDown();
this.label45 = new System.Windows.Forms.Label();
this.STB_RLL_P = new System.Windows.Forms.DomainUpDown();
this.STB_RLL_P = new System.Windows.Forms.NumericUpDown();
this.label46 = new System.Windows.Forms.Label();
this.groupBox23 = new System.Windows.Forms.GroupBox();
this.RATE_YAW_IMAX = new System.Windows.Forms.DomainUpDown();
this.RATE_YAW_IMAX = new System.Windows.Forms.NumericUpDown();
this.label47 = new System.Windows.Forms.Label();
this.RATE_YAW_I = new System.Windows.Forms.DomainUpDown();
this.RATE_YAW_I = new System.Windows.Forms.NumericUpDown();
this.label77 = new System.Windows.Forms.Label();
this.RATE_YAW_P = new System.Windows.Forms.DomainUpDown();
this.RATE_YAW_P = new System.Windows.Forms.NumericUpDown();
this.label82 = new System.Windows.Forms.Label();
this.groupBox24 = new System.Windows.Forms.GroupBox();
this.RATE_PIT_IMAX = new System.Windows.Forms.DomainUpDown();
this.RATE_PIT_IMAX = new System.Windows.Forms.NumericUpDown();
this.label84 = new System.Windows.Forms.Label();
this.RATE_PIT_I = new System.Windows.Forms.DomainUpDown();
this.RATE_PIT_I = new System.Windows.Forms.NumericUpDown();
this.label86 = new System.Windows.Forms.Label();
this.RATE_PIT_P = new System.Windows.Forms.DomainUpDown();
this.RATE_PIT_P = new System.Windows.Forms.NumericUpDown();
this.label87 = new System.Windows.Forms.Label();
this.groupBox25 = new System.Windows.Forms.GroupBox();
this.RATE_RLL_IMAX = new System.Windows.Forms.DomainUpDown();
this.RATE_RLL_IMAX = new System.Windows.Forms.NumericUpDown();
this.label88 = new System.Windows.Forms.Label();
this.RATE_RLL_I = new System.Windows.Forms.DomainUpDown();
this.RATE_RLL_I = new System.Windows.Forms.NumericUpDown();
this.label90 = new System.Windows.Forms.Label();
this.RATE_RLL_P = new System.Windows.Forms.DomainUpDown();
this.RATE_RLL_P = new System.Windows.Forms.NumericUpDown();
this.label91 = new System.Windows.Forms.Label();
this.TabPlanner = new System.Windows.Forms.TabPage();
this.label26 = new System.Windows.Forms.Label();
@ -276,18 +276,18 @@
this.toolTip1 = new System.Windows.Forms.ToolTip(this.components);
this.BUT_compare = new ArdupilotMega.MyButton();
this.groupBox17 = new System.Windows.Forms.GroupBox();
this.ACRO_PIT_IMAX = new System.Windows.Forms.DomainUpDown();
this.ACRO_PIT_IMAX = new System.Windows.Forms.NumericUpDown();
this.label27 = new System.Windows.Forms.Label();
this.ACRO_PIT_I = new System.Windows.Forms.DomainUpDown();
this.ACRO_PIT_I = new System.Windows.Forms.NumericUpDown();
this.label29 = new System.Windows.Forms.Label();
this.ACRO_PIT_P = new System.Windows.Forms.DomainUpDown();
this.ACRO_PIT_P = new System.Windows.Forms.NumericUpDown();
this.label33 = new System.Windows.Forms.Label();
this.groupBox18 = new System.Windows.Forms.GroupBox();
this.ACRO_RLL_IMAX = new System.Windows.Forms.DomainUpDown();
this.ACRO_RLL_IMAX = new System.Windows.Forms.NumericUpDown();
this.label40 = new System.Windows.Forms.Label();
this.ACRO_RLL_I = new System.Windows.Forms.DomainUpDown();
this.ACRO_RLL_I = new System.Windows.Forms.NumericUpDown();
this.label44 = new System.Windows.Forms.Label();
this.ACRO_RLL_P = new System.Windows.Forms.DomainUpDown();
this.ACRO_RLL_P = new System.Windows.Forms.NumericUpDown();
this.label48 = new System.Windows.Forms.Label();
((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit();
this.ConfigTabs.SuspendLayout();
@ -2086,174 +2086,174 @@
private System.Windows.Forms.TabPage TabAPM2;
private System.Windows.Forms.TabPage TabAC2;
private System.Windows.Forms.GroupBox groupBox3;
private System.Windows.Forms.DomainUpDown THR_FS_VALUE;
private System.Windows.Forms.NumericUpDown THR_FS_VALUE;
private System.Windows.Forms.Label label5;
private System.Windows.Forms.DomainUpDown THR_MAX;
private System.Windows.Forms.NumericUpDown THR_MAX;
private System.Windows.Forms.Label label6;
private System.Windows.Forms.DomainUpDown THR_MIN;
private System.Windows.Forms.NumericUpDown THR_MIN;
private System.Windows.Forms.Label label7;
private System.Windows.Forms.DomainUpDown TRIM_THROTTLE;
private System.Windows.Forms.NumericUpDown TRIM_THROTTLE;
private System.Windows.Forms.Label label8;
private System.Windows.Forms.GroupBox groupBox1;
private System.Windows.Forms.DomainUpDown ARSPD_RATIO;
private System.Windows.Forms.NumericUpDown ARSPD_RATIO;
private System.Windows.Forms.Label label1;
private System.Windows.Forms.DomainUpDown ARSPD_FBW_MAX;
private System.Windows.Forms.NumericUpDown ARSPD_FBW_MAX;
private System.Windows.Forms.Label label2;
private System.Windows.Forms.DomainUpDown ARSPD_FBW_MIN;
private System.Windows.Forms.NumericUpDown ARSPD_FBW_MIN;
private System.Windows.Forms.Label label3;
private System.Windows.Forms.DomainUpDown TRIM_ARSPD_CM;
private System.Windows.Forms.NumericUpDown TRIM_ARSPD_CM;
private System.Windows.Forms.Label label4;
private System.Windows.Forms.GroupBox groupBox2;
private System.Windows.Forms.DomainUpDown LIM_PITCH_MIN;
private System.Windows.Forms.NumericUpDown LIM_PITCH_MIN;
private System.Windows.Forms.Label label39;
private System.Windows.Forms.DomainUpDown LIM_PITCH_MAX;
private System.Windows.Forms.NumericUpDown LIM_PITCH_MAX;
private System.Windows.Forms.Label label38;
private System.Windows.Forms.DomainUpDown LIM_ROLL_CD;
private System.Windows.Forms.NumericUpDown LIM_ROLL_CD;
private System.Windows.Forms.Label label37;
private System.Windows.Forms.GroupBox groupBox15;
private System.Windows.Forms.DomainUpDown XTRK_ANGLE_CD;
private System.Windows.Forms.NumericUpDown XTRK_ANGLE_CD;
private System.Windows.Forms.Label label79;
private System.Windows.Forms.DomainUpDown XTRK_GAIN_SC;
private System.Windows.Forms.NumericUpDown XTRK_GAIN_SC;
private System.Windows.Forms.Label label80;
private System.Windows.Forms.GroupBox groupBox16;
private System.Windows.Forms.DomainUpDown KFF_PTCH2THR;
private System.Windows.Forms.NumericUpDown KFF_PTCH2THR;
private System.Windows.Forms.Label label83;
private System.Windows.Forms.DomainUpDown KFF_RDDRMIX;
private System.Windows.Forms.NumericUpDown KFF_RDDRMIX;
private System.Windows.Forms.Label label78;
private System.Windows.Forms.DomainUpDown KFF_PTCHCOMP;
private System.Windows.Forms.NumericUpDown KFF_PTCHCOMP;
private System.Windows.Forms.Label label81;
private System.Windows.Forms.GroupBox groupBox14;
private System.Windows.Forms.DomainUpDown ENRGY2THR_IMAX;
private System.Windows.Forms.NumericUpDown ENRGY2THR_IMAX;
private System.Windows.Forms.Label label73;
private System.Windows.Forms.DomainUpDown ENRGY2THR_D;
private System.Windows.Forms.NumericUpDown ENRGY2THR_D;
private System.Windows.Forms.Label label74;
private System.Windows.Forms.DomainUpDown ENRGY2THR_I;
private System.Windows.Forms.NumericUpDown ENRGY2THR_I;
private System.Windows.Forms.Label label75;
private System.Windows.Forms.DomainUpDown ENRGY2THR_P;
private System.Windows.Forms.NumericUpDown ENRGY2THR_P;
private System.Windows.Forms.Label label76;
private System.Windows.Forms.GroupBox groupBox13;
private System.Windows.Forms.DomainUpDown ALT2PTCH_IMAX;
private System.Windows.Forms.NumericUpDown ALT2PTCH_IMAX;
private System.Windows.Forms.Label label69;
private System.Windows.Forms.DomainUpDown ALT2PTCH_D;
private System.Windows.Forms.NumericUpDown ALT2PTCH_D;
private System.Windows.Forms.Label label70;
private System.Windows.Forms.DomainUpDown ALT2PTCH_I;
private System.Windows.Forms.NumericUpDown ALT2PTCH_I;
private System.Windows.Forms.Label label71;
private System.Windows.Forms.DomainUpDown ALT2PTCH_P;
private System.Windows.Forms.NumericUpDown ALT2PTCH_P;
private System.Windows.Forms.Label label72;
private System.Windows.Forms.GroupBox groupBox12;
private System.Windows.Forms.DomainUpDown ARSP2PTCH_IMAX;
private System.Windows.Forms.NumericUpDown ARSP2PTCH_IMAX;
private System.Windows.Forms.Label label65;
private System.Windows.Forms.DomainUpDown ARSP2PTCH_D;
private System.Windows.Forms.NumericUpDown ARSP2PTCH_D;
private System.Windows.Forms.Label label66;
private System.Windows.Forms.DomainUpDown ARSP2PTCH_I;
private System.Windows.Forms.NumericUpDown ARSP2PTCH_I;
private System.Windows.Forms.Label label67;
private System.Windows.Forms.DomainUpDown ARSP2PTCH_P;
private System.Windows.Forms.NumericUpDown ARSP2PTCH_P;
private System.Windows.Forms.Label label68;
private System.Windows.Forms.GroupBox groupBox11;
private System.Windows.Forms.DomainUpDown HDNG2RLL_IMAX;
private System.Windows.Forms.NumericUpDown HDNG2RLL_IMAX;
private System.Windows.Forms.Label label61;
private System.Windows.Forms.DomainUpDown HDNG2RLL_D;
private System.Windows.Forms.NumericUpDown HDNG2RLL_D;
private System.Windows.Forms.Label label62;
private System.Windows.Forms.DomainUpDown HDNG2RLL_I;
private System.Windows.Forms.NumericUpDown HDNG2RLL_I;
private System.Windows.Forms.Label label63;
private System.Windows.Forms.DomainUpDown HDNG2RLL_P;
private System.Windows.Forms.NumericUpDown HDNG2RLL_P;
private System.Windows.Forms.Label label64;
private System.Windows.Forms.GroupBox groupBox10;
private System.Windows.Forms.DomainUpDown YW2SRV_IMAX;
private System.Windows.Forms.NumericUpDown YW2SRV_IMAX;
private System.Windows.Forms.Label label57;
private System.Windows.Forms.DomainUpDown YW2SRV_D;
private System.Windows.Forms.NumericUpDown YW2SRV_D;
private System.Windows.Forms.Label label58;
private System.Windows.Forms.DomainUpDown YW2SRV_I;
private System.Windows.Forms.NumericUpDown YW2SRV_I;
private System.Windows.Forms.Label label59;
private System.Windows.Forms.DomainUpDown YW2SRV_P;
private System.Windows.Forms.NumericUpDown YW2SRV_P;
private System.Windows.Forms.Label label60;
private System.Windows.Forms.GroupBox groupBox9;
private System.Windows.Forms.DomainUpDown PTCH2SRV_IMAX;
private System.Windows.Forms.NumericUpDown PTCH2SRV_IMAX;
private System.Windows.Forms.Label label53;
private System.Windows.Forms.DomainUpDown PTCH2SRV_D;
private System.Windows.Forms.NumericUpDown PTCH2SRV_D;
private System.Windows.Forms.Label label54;
private System.Windows.Forms.DomainUpDown PTCH2SRV_I;
private System.Windows.Forms.NumericUpDown PTCH2SRV_I;
private System.Windows.Forms.Label label55;
private System.Windows.Forms.DomainUpDown PTCH2SRV_P;
private System.Windows.Forms.NumericUpDown PTCH2SRV_P;
private System.Windows.Forms.Label label56;
private System.Windows.Forms.GroupBox groupBox8;
private System.Windows.Forms.DomainUpDown RLL2SRV_IMAX;
private System.Windows.Forms.NumericUpDown RLL2SRV_IMAX;
private System.Windows.Forms.Label label49;
private System.Windows.Forms.DomainUpDown RLL2SRV_D;
private System.Windows.Forms.NumericUpDown RLL2SRV_D;
private System.Windows.Forms.Label label50;
private System.Windows.Forms.DomainUpDown RLL2SRV_I;
private System.Windows.Forms.NumericUpDown RLL2SRV_I;
private System.Windows.Forms.Label label51;
private System.Windows.Forms.DomainUpDown RLL2SRV_P;
private System.Windows.Forms.NumericUpDown RLL2SRV_P;
private System.Windows.Forms.Label label52;
private System.Windows.Forms.GroupBox groupBox4;
private System.Windows.Forms.DomainUpDown NAV_LAT_IMAX;
private System.Windows.Forms.NumericUpDown NAV_LAT_IMAX;
private System.Windows.Forms.Label label13;
private System.Windows.Forms.DomainUpDown NAV_LAT_I;
private System.Windows.Forms.NumericUpDown NAV_LAT_I;
private System.Windows.Forms.Label label15;
private System.Windows.Forms.DomainUpDown NAV_LAT_P;
private System.Windows.Forms.NumericUpDown NAV_LAT_P;
private System.Windows.Forms.Label label16;
private System.Windows.Forms.GroupBox groupBox6;
private System.Windows.Forms.DomainUpDown XTRACK_IMAX;
private System.Windows.Forms.NumericUpDown XTRACK_IMAX;
private System.Windows.Forms.Label label11;
private System.Windows.Forms.DomainUpDown XTRACK_I;
private System.Windows.Forms.NumericUpDown XTRACK_I;
private System.Windows.Forms.Label label17;
private System.Windows.Forms.DomainUpDown XTRACK_P;
private System.Windows.Forms.NumericUpDown XTRACK_P;
private System.Windows.Forms.Label label18;
private System.Windows.Forms.GroupBox groupBox7;
private System.Windows.Forms.DomainUpDown THR_ALT_IMAX;
private System.Windows.Forms.NumericUpDown THR_ALT_IMAX;
private System.Windows.Forms.Label label19;
private System.Windows.Forms.DomainUpDown THR_ALT_I;
private System.Windows.Forms.NumericUpDown THR_ALT_I;
private System.Windows.Forms.Label label21;
private System.Windows.Forms.DomainUpDown THR_ALT_P;
private System.Windows.Forms.NumericUpDown THR_ALT_P;
private System.Windows.Forms.Label label22;
private System.Windows.Forms.GroupBox groupBox19;
private System.Windows.Forms.DomainUpDown HLD_LAT_IMAX;
private System.Windows.Forms.NumericUpDown HLD_LAT_IMAX;
private System.Windows.Forms.Label label28;
private System.Windows.Forms.DomainUpDown HLD_LAT_I;
private System.Windows.Forms.NumericUpDown HLD_LAT_I;
private System.Windows.Forms.Label label30;
private System.Windows.Forms.DomainUpDown HLD_LAT_P;
private System.Windows.Forms.NumericUpDown HLD_LAT_P;
private System.Windows.Forms.Label label31;
private System.Windows.Forms.GroupBox groupBox20;
private System.Windows.Forms.DomainUpDown STB_YAW_IMAX;
private System.Windows.Forms.NumericUpDown STB_YAW_IMAX;
private System.Windows.Forms.Label label32;
private System.Windows.Forms.DomainUpDown STB_YAW_I;
private System.Windows.Forms.NumericUpDown STB_YAW_I;
private System.Windows.Forms.Label label34;
private System.Windows.Forms.DomainUpDown STB_YAW_P;
private System.Windows.Forms.NumericUpDown STB_YAW_P;
private System.Windows.Forms.Label label35;
private System.Windows.Forms.GroupBox groupBox21;
private System.Windows.Forms.DomainUpDown STB_PIT_IMAX;
private System.Windows.Forms.NumericUpDown STB_PIT_IMAX;
private System.Windows.Forms.Label label36;
private System.Windows.Forms.DomainUpDown STB_PIT_I;
private System.Windows.Forms.NumericUpDown STB_PIT_I;
private System.Windows.Forms.Label label41;
private System.Windows.Forms.DomainUpDown STB_PIT_P;
private System.Windows.Forms.NumericUpDown STB_PIT_P;
private System.Windows.Forms.Label label42;
private System.Windows.Forms.GroupBox groupBox22;
private System.Windows.Forms.DomainUpDown STB_RLL_IMAX;
private System.Windows.Forms.NumericUpDown STB_RLL_IMAX;
private System.Windows.Forms.Label label43;
private System.Windows.Forms.DomainUpDown STB_RLL_I;
private System.Windows.Forms.NumericUpDown STB_RLL_I;
private System.Windows.Forms.Label label45;
private System.Windows.Forms.DomainUpDown STB_RLL_P;
private System.Windows.Forms.NumericUpDown STB_RLL_P;
private System.Windows.Forms.Label label46;
private System.Windows.Forms.GroupBox groupBox23;
private System.Windows.Forms.DomainUpDown RATE_YAW_IMAX;
private System.Windows.Forms.NumericUpDown RATE_YAW_IMAX;
private System.Windows.Forms.Label label47;
private System.Windows.Forms.DomainUpDown RATE_YAW_I;
private System.Windows.Forms.NumericUpDown RATE_YAW_I;
private System.Windows.Forms.Label label77;
private System.Windows.Forms.DomainUpDown RATE_YAW_P;
private System.Windows.Forms.NumericUpDown RATE_YAW_P;
private System.Windows.Forms.Label label82;
private System.Windows.Forms.GroupBox groupBox24;
private System.Windows.Forms.DomainUpDown RATE_PIT_IMAX;
private System.Windows.Forms.NumericUpDown RATE_PIT_IMAX;
private System.Windows.Forms.Label label84;
private System.Windows.Forms.DomainUpDown RATE_PIT_I;
private System.Windows.Forms.NumericUpDown RATE_PIT_I;
private System.Windows.Forms.Label label86;
private System.Windows.Forms.DomainUpDown RATE_PIT_P;
private System.Windows.Forms.NumericUpDown RATE_PIT_P;
private System.Windows.Forms.Label label87;
private System.Windows.Forms.GroupBox groupBox25;
private System.Windows.Forms.DomainUpDown RATE_RLL_IMAX;
private System.Windows.Forms.NumericUpDown RATE_RLL_IMAX;
private System.Windows.Forms.Label label88;
private System.Windows.Forms.DomainUpDown RATE_RLL_I;
private System.Windows.Forms.NumericUpDown RATE_RLL_I;
private System.Windows.Forms.Label label90;
private System.Windows.Forms.DomainUpDown RATE_RLL_P;
private System.Windows.Forms.NumericUpDown RATE_RLL_P;
private System.Windows.Forms.Label label91;
private System.Windows.Forms.TabPage TabPlanner;
private System.Windows.Forms.Label label92;
@ -2293,9 +2293,9 @@
private System.Windows.Forms.CheckBox CHK_resetapmonconnect;
private System.Windows.Forms.Label label108;
private System.Windows.Forms.CheckBox CHK_lockrollpitch;
private System.Windows.Forms.DomainUpDown WP_SPEED_MAX;
private System.Windows.Forms.NumericUpDown WP_SPEED_MAX;
private System.Windows.Forms.Label label9;
private System.Windows.Forms.DomainUpDown XTRK_ANGLE_CD1;
private System.Windows.Forms.NumericUpDown XTRK_ANGLE_CD1;
private System.Windows.Forms.Label label10;
private System.Windows.Forms.CheckBox CHK_speechaltwarning;
private System.Windows.Forms.ToolTip toolTip1;
@ -2312,28 +2312,28 @@
private System.Windows.Forms.Label label12;
private System.Windows.Forms.CheckBox CHK_GDIPlus;
private System.Windows.Forms.GroupBox groupBox5;
private System.Windows.Forms.DomainUpDown THR_RATE_IMAX;
private System.Windows.Forms.DomainUpDown THR_RATE_I;
private System.Windows.Forms.NumericUpDown THR_RATE_IMAX;
private System.Windows.Forms.NumericUpDown THR_RATE_I;
private System.Windows.Forms.Label label20;
private System.Windows.Forms.DomainUpDown THR_RATE_P;
private System.Windows.Forms.NumericUpDown THR_RATE_P;
private System.Windows.Forms.Label label25;
private System.Windows.Forms.ComboBox CMB_videoresolutions;
private System.Windows.Forms.Label label109;
private System.Windows.Forms.Label label14;
private System.Windows.Forms.Label label26;
private System.Windows.Forms.GroupBox groupBox17;
private System.Windows.Forms.DomainUpDown ACRO_PIT_IMAX;
private System.Windows.Forms.NumericUpDown ACRO_PIT_IMAX;
private System.Windows.Forms.Label label27;
private System.Windows.Forms.DomainUpDown ACRO_PIT_I;
private System.Windows.Forms.NumericUpDown ACRO_PIT_I;
private System.Windows.Forms.Label label29;
private System.Windows.Forms.DomainUpDown ACRO_PIT_P;
private System.Windows.Forms.NumericUpDown ACRO_PIT_P;
private System.Windows.Forms.Label label33;
private System.Windows.Forms.GroupBox groupBox18;
private System.Windows.Forms.DomainUpDown ACRO_RLL_IMAX;
private System.Windows.Forms.NumericUpDown ACRO_RLL_IMAX;
private System.Windows.Forms.Label label40;
private System.Windows.Forms.DomainUpDown ACRO_RLL_I;
private System.Windows.Forms.NumericUpDown ACRO_RLL_I;
private System.Windows.Forms.Label label44;
private System.Windows.Forms.DomainUpDown ACRO_RLL_P;
private System.Windows.Forms.NumericUpDown ACRO_RLL_P;
private System.Windows.Forms.Label label48;
}
}

77
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs
View File

@ -61,6 +61,17 @@ namespace ArdupilotMega.GCSViews
{
InitializeComponent();
//this.Width = this.Parent.Width;
//this.Height = this.Parent.Height;
// fix for dup name
XTRK_ANGLE_CD1.Name = "XTRK_ANGLE_CD";
}
private void Configuration_Load(object sender, EventArgs e)
{
startup = true;
// enable disable relevbant hardware tabs
if (MainV2.APMFirmware == MainV2.Firmwares.ArduPlane)
{
@ -83,12 +94,8 @@ namespace ArdupilotMega.GCSViews
param = MainV2.comPort.param;
processToScreen();
// fix for dup name
XTRK_ANGLE_CD1.Name = "XTRK_ANGLE_CD";
}
private void Configuration_Load(object sender, EventArgs e)
{
// setup up camera button states
if (MainV2.cam != null)
{
@ -285,6 +292,7 @@ namespace ArdupilotMega.GCSViews
internal void processToScreen()
{
toolTip1.RemoveAll();
Params.Rows.Clear();
// process hashdefines and update display
@ -303,12 +311,12 @@ namespace ArdupilotMega.GCSViews
if (tooltips[value] != null)
{
Params.Rows[Params.RowCount - 1].Cells[Command.Index].ToolTipText = ((paramsettings)tooltips[value]).desc;
Params.Rows[Params.RowCount - 1].Cells[RawValue.Index].ToolTipText = ((paramsettings)tooltips[value]).desc;
//Params.Rows[Params.RowCount - 1].Cells[RawValue.Index].ToolTipText = ((paramsettings)tooltips[value]).desc;
Params.Rows[Params.RowCount - 1].Cells[Value.Index].ToolTipText = ((paramsettings)tooltips[value]).desc;
Params.Rows[Params.RowCount - 1].Cells[Default.Index].Value = ((paramsettings)tooltips[value]).normalvalue;
Params.Rows[Params.RowCount - 1].Cells[mavScale.Index].Value = ((paramsettings)tooltips[value]).scale;
Params.Rows[Params.RowCount - 1].Cells[Value.Index].Value = float.Parse(Params.Rows[Params.RowCount - 1].Cells[RawValue.Index].Value.ToString()) / float.Parse(Params.Rows[Params.RowCount - 1].Cells[mavScale.Index].Value.ToString());
//Params.Rows[Params.RowCount - 1].Cells[Default.Index].Value = ((paramsettings)tooltips[value]).normalvalue;
//Params.Rows[Params.RowCount - 1].Cells[mavScale.Index].Value = ((paramsettings)tooltips[value]).scale;
//Params.Rows[Params.RowCount - 1].Cells[Value.Index].Value = float.Parse(Params.Rows[Params.RowCount - 1].Cells[RawValue.Index].Value.ToString()) / float.Parse(Params.Rows[Params.RowCount - 1].Cells[mavScale.Index].Value.ToString());
}
}
catch { }
@ -319,20 +327,23 @@ namespace ArdupilotMega.GCSViews
{
try
{
((DomainUpDown)ctl).Items.AddRange(genpids());
string option = ((float)param[value]).ToString("0.0##");
int index = ((DomainUpDown)ctl).Items.IndexOf(option);
((DomainUpDown)ctl).BackColor = Color.FromArgb(0x43, 0x44, 0x45);
Console.WriteLine(name + " " + option + " " + index + " " + ((float)param[value]));
((DomainUpDown)ctl).Validated += null;
if (index == -1)
NumericUpDown thisctl = ((NumericUpDown)ctl);
thisctl.Maximum = 9000;
thisctl.Minimum = -9000;
thisctl.Value = (decimal)(float)param[value];
thisctl.Increment = (decimal)0.001;
if (thisctl.Name.EndsWith("_P") || thisctl.Name.EndsWith("_I") || thisctl.Name.EndsWith("_D") || thisctl.Value == 0 || thisctl.Value.ToString("0.###", new System.Globalization.CultureInfo("en-US")).Contains("."))
{
((DomainUpDown)ctl).Text = ((float)param[value]).ToString("0.0##");
thisctl.DecimalPlaces = 3;
}
else
{
((DomainUpDown)ctl).SelectedIndex = index;
thisctl.Increment = (decimal)1;
thisctl.DecimalPlaces = 1;
}
thisctl.BackColor = Color.FromArgb(0x43, 0x44, 0x45);
thisctl.Validated += null;
if (tooltips[value] != null)
{
try
@ -341,7 +352,7 @@ namespace ArdupilotMega.GCSViews
}
catch { }
}
((DomainUpDown)ctl).Validated += new EventHandler(EEPROM_View_float_TextChanged);
thisctl.Validated += new EventHandler(EEPROM_View_float_TextChanged);
}
catch { }
@ -475,27 +486,18 @@ namespace ArdupilotMega.GCSViews
Params[e.ColumnIndex, e.RowIndex].Style.BackColor = Color.Red;
}
// set control as well
Control[] text = this.Controls.Find(Params[0, e.RowIndex].Value.ToString(), true);
try
{
// set control as well
Control[] text = this.Controls.Find(Params[0, e.RowIndex].Value.ToString(), true);
if (text.Length > 0)
{
string option = (float.Parse(Params[e.ColumnIndex, e.RowIndex].Value.ToString())).ToString("0.0##", System.Globalization.CultureInfo.CurrentCulture);
int index = ((DomainUpDown)text[0]).Items.IndexOf(option);
if (index != -1)
{
((DomainUpDown)text[0]).SelectedIndex = index;
((DomainUpDown)text[0]).BackColor = Color.Green;
}
else
{
((DomainUpDown)text[0]).Text = option;
((DomainUpDown)text[0]).BackColor = Color.Green;
}
decimal option = (decimal)(float.Parse(Params[e.ColumnIndex, e.RowIndex].Value.ToString()));
((NumericUpDown)text[0]).Value = option;
((NumericUpDown)text[0]).BackColor = Color.Green;
}
}
catch { }
catch { ((NumericUpDown)text[0]).BackColor = Color.Red; }
Params.Focus();
}
@ -521,8 +523,10 @@ namespace ArdupilotMega.GCSViews
int index = line.IndexOf(',');
int index2 = line.IndexOf(',', index + 1);
if (index == -1)
continue;
if (index2 != -1)
line = line.Replace(',','.');
@ -600,7 +604,7 @@ namespace ArdupilotMega.GCSViews
Control[] text = this.Controls.Find(value, true);
if (text.Length > 0)
{
((DomainUpDown)text[0]).BackColor = Color.FromArgb(0x43, 0x44, 0x45);
((NumericUpDown)text[0]).BackColor = Color.FromArgb(0x43, 0x44, 0x45);
}
}
catch { }
@ -845,10 +849,7 @@ namespace ArdupilotMega.GCSViews
((MyButton)sender).Enabled = true;
startup = true;
param = MainV2.comPort.param;
processToScreen();
Configuration_Load(null, null);
startup = false;
}
private void CHK_speechbattery_CheckedChanged(object sender, EventArgs e)

46
Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
View File

@ -112,6 +112,13 @@ namespace ArdupilotMega.GCSViews
List<string> list = new List<string>();
//foreach (object obj in Enum.GetValues(typeof(MAVLink.MAV_ACTION)))
#if MAVLINK10
{
list.Add("LOITER_UNLIM");
list.Add("RETURN_TO_LAUNCH");
list.Add("PREFLIGHT_CALIBRATION");
}
#else
{
list.Add("RETURN");
list.Add("HALT");
@ -126,6 +133,7 @@ namespace ArdupilotMega.GCSViews
list.Add("TAKEOFF");
list.Add("CALIBRATE_GYRO");
}
#endif
CMB_action.DataSource = list;
@ -364,7 +372,7 @@ namespace ArdupilotMega.GCSViews
// if (CB_tuning.Checked == false) // draw if in view
// if (CB_tuning.Checked == false) // draw if in view
{
if (MainV2.comPort.logreadmode && MainV2.comPort.logplaybackfile != null)
@ -413,7 +421,7 @@ namespace ArdupilotMega.GCSViews
if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane)
{
routes.Markers.Add(new GMapMarkerPlane(currentloc, MainV2.cs.yaw, MainV2.cs.groundcourse, MainV2.cs.nav_bearing,MainV2.cs.target_bearing));
routes.Markers.Add(new GMapMarkerPlane(currentloc, MainV2.cs.yaw, MainV2.cs.groundcourse, MainV2.cs.nav_bearing, MainV2.cs.target_bearing));
}
else
{
@ -445,7 +453,7 @@ namespace ArdupilotMega.GCSViews
tracklast = DateTime.Now;
}
}
catch (Exception ex) { Console.WriteLine("FD Main loop exception "+ ex.ToString()); }
catch (Exception ex) { Console.WriteLine("FD Main loop exception " + ex.ToString()); }
}
Console.WriteLine("FD Main loop exit");
}
@ -684,7 +692,11 @@ namespace ArdupilotMega.GCSViews
try
{
((Button)sender).Enabled = false;
#if MAVLINK10
comPort.doCommand((MAVLink.MAV_CMD)Enum.Parse(typeof(MAVLink.MAV_CMD), CMB_action.Text));
#else
comPort.doAction((MAVLink.MAV_ACTION)Enum.Parse(typeof(MAVLink.MAV_ACTION), "MAV_ACTION_" + CMB_action.Text));
#endif
}
catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true;
@ -961,20 +973,7 @@ namespace ArdupilotMega.GCSViews
private void BUT_setmode_Click(object sender, EventArgs e)
{
MAVLink.__mavlink_set_nav_mode_t navmode = new MAVLink.__mavlink_set_nav_mode_t();
MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t();
if (Common.translateMode(CMB_modes.Text, ref navmode, ref mode))
{
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode);
System.Threading.Thread.Sleep(10);
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode);
System.Threading.Thread.Sleep(10);
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
System.Threading.Thread.Sleep(10);
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
}
MainV2.comPort.setMode(CMB_modes.Text);
}
private void BUT_setwp_Click(object sender, EventArgs e)
@ -1009,7 +1008,11 @@ namespace ArdupilotMega.GCSViews
try
{
((Button)sender).Enabled = false;
#if MAVLINK10
MainV2.comPort.setMode("AUTO");
#else
comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_SET_AUTO);
#endif
}
catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true;
@ -1020,7 +1023,11 @@ namespace ArdupilotMega.GCSViews
try
{
((Button)sender).Enabled = false;
#if MAVLINK10
MainV2.comPort.setMode("RTL");
#else
comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_RETURN);
#endif
}
catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true;
@ -1031,7 +1038,11 @@ namespace ArdupilotMega.GCSViews
try
{
((Button)sender).Enabled = false;
#if MAVLINK10
MainV2.comPort.setMode("MANUAL");
#else
comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_SET_MANUAL);
#endif
}
catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true;
@ -1510,6 +1521,7 @@ namespace ArdupilotMega.GCSViews
MainV2.comPort.setMountConfigure(MAVLink.MAV_MOUNT_MODE.MAV_MOUNT_MODE_GPS_POINT, true, true, true);
MainV2.comPort.setMountControl(gotolocation.Lat, gotolocation.Lng, (int)(intalt / MainV2.cs.multiplierdist), true);
}
}
}

21
Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs
View File

@ -29,6 +29,7 @@ namespace ArdupilotMega.GCSViews
bool quickadd = false;
bool isonline = true;
bool sethome = false;
bool polygongridmode = false;
Hashtable param = new Hashtable();
public static Hashtable hashdefines = new Hashtable();
public static List<PointLatLngAlt> pointlist = new List<PointLatLngAlt>(); // used to calc distance
@ -370,6 +371,11 @@ namespace ArdupilotMega.GCSViews
/// <param name="alt"></param>
public void callMe(double lat, double lng, int alt)
{
if (polygongridmode)
{
addPolygonPointToolStripMenuItem_Click(null, null);
return;
}
if (sethome)
{
@ -2171,6 +2177,8 @@ namespace ArdupilotMega.GCSViews
private void BUT_grid_Click(object sender, EventArgs e)
{
polygongridmode = false;
if (drawnpolygon == null || drawnpolygon.Points.Count == 0)
{
MessageBox.Show("Right click the map to draw a polygon", "Area", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
@ -2366,8 +2374,8 @@ namespace ArdupilotMega.GCSViews
}
}
drawnpolygon.Points.Clear();
drawnpolygons.Markers.Clear();
//drawnpolygon.Points.Clear();
//drawnpolygons.Markers.Clear();
MainMap.Refresh();
}
@ -2460,6 +2468,13 @@ namespace ArdupilotMega.GCSViews
private void addPolygonPointToolStripMenuItem_Click(object sender, EventArgs e)
{
if (polygongridmode == false)
{
MessageBox.Show("You will remain in polygon mode until you clear the polygon or create a grid");
}
polygongridmode = true;
List < PointLatLng > polygonPoints = new List<PointLatLng>();
if (drawnpolygons.Polygons.Count == 0)
{
@ -2479,6 +2494,7 @@ namespace ArdupilotMega.GCSViews
private void clearPolygonToolStripMenuItem_Click(object sender, EventArgs e)
{
polygongridmode = false;
if (drawnpolygon == null)
return;
drawnpolygon.Points.Clear();
@ -2591,6 +2607,7 @@ namespace ArdupilotMega.GCSViews
setfromGE(end.Lat, end.Lng, (int)float.Parse(TXT_DefaultAlt.Text));
}
private void BUT_Camera_Click(object sender, EventArgs e)
{
Camera form = new Camera();

154
Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
View File

@ -684,6 +684,9 @@ namespace ArdupilotMega.GCSViews
float oldax =0, olday =0, oldaz = 0;
DateTime oldtime = DateTime.Now;
#if MAVLINK10
ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t oldgps = new MAVLink.__mavlink_gps_raw_int_t();
#endif
ArdupilotMega.MAVLink.__mavlink_attitude_t oldatt = new ArdupilotMega.MAVLink.__mavlink_attitude_t();
@ -695,10 +698,15 @@ namespace ArdupilotMega.GCSViews
/// <param name="comPort">Com Port</param>
private void RECVprocess(byte[] data, int receviedbytes, ArdupilotMega.MAVLink comPort)
{
#if MAVLINK10
ArdupilotMega.MAVLink.__mavlink_hil_state_t hilstate = new ArdupilotMega.MAVLink.__mavlink_hil_state_t();
ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t();
#else
ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t();
#endif
ArdupilotMega.MAVLink.__mavlink_raw_imu_t imu = new ArdupilotMega.MAVLink.__mavlink_raw_imu_t();
ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t();
ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();
@ -742,7 +750,7 @@ 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;
@ -775,8 +783,11 @@ namespace ArdupilotMega.GCSViews
oldaz = DATA[4][4];
double head = DATA[18][2] - 90;
imu.usec = ((ulong)DateTime.Now.ToBinary());
#if MAVLINK10
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
#else
imu.usec = ((ulong)DateTime.Now.ToBinary());
#endif
imu.xgyro = xgyro; // roll - yes
imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
imu.ygyro = ygyro; // pitch - yes
@ -789,7 +800,15 @@ namespace ArdupilotMega.GCSViews
imu.zacc = (Int16)(accel3D.Z * 1000);
//Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc);
#if MAVLINK10
gps.alt = (int)(DATA[20][2] * ft2m * 1000);
gps.fix_type = 3;
gps.cog = (ushort)(DATA[19][2] * 100);
gps.lat = (int)(DATA[20][0] * 1.0e7);
gps.lon = (int)(DATA[20][1] * 1.0e7);
gps.time_usec = ((ulong)0);
gps.vel = (ushort)(DATA[3][7] * 0.44704 * 100);
#else
gps.alt = ((float)(DATA[20][2] * ft2m));
gps.fix_type = 3;
gps.hdg = ((float)DATA[19][2]);
@ -797,8 +816,7 @@ namespace ArdupilotMega.GCSViews
gps.lon = ((float)DATA[20][1]);
gps.usec = ((ulong)0);
gps.v = ((float)(DATA[3][7] * 0.44704));
gps.eph = 0;
gps.epv = 0;
#endif
asp.airspeed = ((float)(DATA[3][6] * 0.44704));
@ -826,7 +844,12 @@ namespace ArdupilotMega.GCSViews
chkSensor.Checked = true;
imu.usec = ((ulong)DateTime.Now.Ticks);
#if MAVLINK10
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
#else
imu.usec = ((ulong)DateTime.Now.ToBinary());
#endif
imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2));
imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
imu.xmag = 0;
@ -837,6 +860,15 @@ namespace ArdupilotMega.GCSViews
imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293));
imu.zmag = 0;
#if MAVLINK10
gps.alt = ((int)(imudata2.altitude * ft2m * 1000));
gps.fix_type = 3;
gps.cog = (ushort)(Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg * 100);
gps.lat = (int)(imudata2.latitude * 1.0e7);
gps.lon = (int)(imudata2.longitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m * 100);
#else
gps.alt = ((float)(imudata2.altitude * ft2m));
gps.fix_type = 3;
gps.hdg = ((float)Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg);
@ -845,6 +877,7 @@ namespace ArdupilotMega.GCSViews
gps.usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m);
#endif
//FileStream stream = File.OpenWrite("fgdata.txt");
//stream.Write(data, 0, receviedbytes);
//stream.Close();
@ -867,7 +900,11 @@ namespace ArdupilotMega.GCSViews
att.yawspeed = (aeroin.Model_fAngVelZ);
imu.usec = ((ulong)DateTime.Now.ToBinary());
#if MAVLINK10
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
#else
imu.usec = ((ulong)DateTime.Now.ToBinary());
#endif
imu.xgyro = (short)(aeroin.Model_fAngVelX * 1000); // roll - yes
//imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
imu.ygyro = (short)(aeroin.Model_fAngVelY * 1000); // pitch - yes
@ -881,9 +918,17 @@ namespace ArdupilotMega.GCSViews
imu.yacc = (Int16)((accel3D.Y + aeroin.Model_fAccelY) * 1000); // roll
imu.zacc = (Int16)((accel3D.Z + aeroin.Model_fAccelZ) * 1000);
Console.WriteLine("x {0} y {1} z {2}",imu.xacc,imu.yacc,imu.zacc);
Console.WriteLine("x {0} y {1} z {2}", imu.xacc, imu.yacc, imu.zacc);
#if MAVLINK10
gps.alt = ((int)(aeroin.Model_fPosZ) * 1000);
gps.fix_type = 3;
gps.cog = (ushort)(Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg * 100);
gps.lat = (int)(aeroin.Model_fLatitude * 1.0e7);
gps.lon = (int)(aeroin.Model_fLongitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX)) * 100);
#else
gps.alt = ((float)(aeroin.Model_fPosZ));
gps.fix_type = 3;
gps.hdg = ((float)Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg);
@ -892,6 +937,7 @@ namespace ArdupilotMega.GCSViews
gps.usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX)));
#endif
float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY;
float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX;
@ -915,7 +961,11 @@ namespace ArdupilotMega.GCSViews
att.pitch = fdm.theta;
att.yaw = fdm.psi;
imu.usec = ((ulong)DateTime.Now.ToBinary());
#if MAVLINK10
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
#else
imu.usec = ((ulong)DateTime.Now.ToBinary());
#endif
imu.xgyro = (short)(fdm.phidot * 1150); // roll - yes
//imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
imu.ygyro = (short)(fdm.thetadot * 1150); // pitch - yes
@ -928,7 +978,15 @@ namespace ArdupilotMega.GCSViews
imu.zacc = (Int16)Math.Min(Int16.MaxValue, Math.Max(Int16.MinValue, (fdm.A_Z_pilot / 32.2 * 9808)));
//Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc);
#if MAVLINK10
gps.alt = ((int)(fdm.altitude * ft2m * 1000));
gps.fix_type = 3;
gps.cog = (ushort)((((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360) * 100);
gps.lat = (int)(fdm.latitude * rad2deg * 1.0e7);
gps.lon = (int)(fdm.longitude * rad2deg * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m * 100);
#else
gps.alt = ((float)(fdm.altitude * ft2m));
gps.fix_type = 3;
gps.hdg = (float)(((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360);
@ -938,6 +996,7 @@ namespace ArdupilotMega.GCSViews
gps.usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m);
#endif
asp.airspeed = fdm.vcas * kts2fps * ft2m;
}
else
@ -994,7 +1053,15 @@ namespace ArdupilotMega.GCSViews
att.pitch = (DATA[18][0]);
att.roll = (DATA[18][1]);
att.yaw = (DATA[19][2]);
#if MAVLINK10
gps.alt = ((int)(DATA[20][2] * ft2m * 1000));
gps.fix_type = 3;
gps.cog = (ushort)(DATA[18][2] * 100);
gps.lat = (int)(DATA[20][0] * 1.0e7);
gps.lon = (int)(DATA[20][1] * 1.0e7);
gps.time_usec = ((ulong)0);
gps.vel = (ushort)((DATA[3][7] * 0.44704 * 100));
#else
gps.alt = ((float)(DATA[20][2] * ft2m));
gps.fix_type = 3;
gps.hdg = ((float)DATA[18][2]);
@ -1002,8 +1069,7 @@ namespace ArdupilotMega.GCSViews
gps.lon = ((float)DATA[20][1]);
gps.usec = ((ulong)0);
gps.v = ((float)(DATA[3][7] * 0.44704));
gps.eph = 0;
gps.epv = 0;
#endif
asp.airspeed = ((float)(DATA[3][6] * 0.44704));
}
@ -1014,25 +1080,62 @@ namespace ArdupilotMega.GCSViews
return;
}
#if MAVLINK10
TimeSpan gpsspan = DateTime.Now - lastgpsupdate;
if (gpsspan.TotalMilliseconds >= GPS_rate)
{
lastgpsupdate = DateTime.Now;
oldgps = gps;
//comPort.sendPacket(gps);
}
hilstate.alt = oldgps.alt;
hilstate.lat = oldgps.lat;
hilstate.lon = oldgps.lon;
hilstate.pitch = att.pitch;
hilstate.pitchspeed = att.pitchspeed;
hilstate.roll = att.roll;
hilstate.rollspeed = att.rollspeed;
hilstate.time_usec = gps.time_usec;
hilstate.vx = (short)(gps.vel * Math.Sin(oldgps.cog / 100.0 * deg2rad));
hilstate.vy = (short)(gps.vel * Math.Cos(oldgps.cog / 100.0 * deg2rad));
hilstate.vz = 0;
hilstate.xacc = imu.xacc;
hilstate.yacc = imu.yacc;
hilstate.yaw = att.yaw;
hilstate.yawspeed = att.yawspeed;
hilstate.zacc = imu.zacc;
comPort.sendPacket(hilstate);
comPort.sendPacket(asp);
#else
if (chkSensor.Checked == false) // attitude
{
comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_ATTITUDE, att);
comPort.sendPacket( att);
comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_VFR_HUD, asp);
comPort.sendPacket( asp);
}
else // raw imu
{
// imudata
comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_RAW_IMU, imu);
comPort.sendPacket( imu);
MAVLink.__mavlink_raw_pressure_t pres = new MAVLink.__mavlink_raw_pressure_t();
double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * gps.alt, 5.25588));
pres.press_diff1 = (short)(int)(calc - 101325); // 0 alt is 0 pa
#endif
comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_RAW_PRESSURE, pres);
MAVLink.__mavlink_raw_pressure_t pres = new MAVLink.__mavlink_raw_pressure_t();
double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * gps.alt, 5.25588)); // updated from valid gps
pres.press_diff1 = (short)(int)(calc - 101325); // 0 alt is 0 pa
comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_VFR_HUD, asp);
comPort.sendPacket(pres);
#if !MAVLINK10
comPort.sendPacket(asp);
}
TimeSpan gpsspan = DateTime.Now - lastgpsupdate;
@ -1041,8 +1144,9 @@ namespace ArdupilotMega.GCSViews
{
lastgpsupdate = DateTime.Now;
comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_GPS_RAW, gps);
comPort.sendPacket(gps);
}
#endif
}
HIL.QuadCopter quad = new HIL.QuadCopter();

64
Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs
View File

@ -47,23 +47,23 @@ namespace ArdupilotMega.HIL
{
float min_pwm = 1000;
float max_pwm = 2000;
//'''scale a PWM value''' # default to servo range of 1000 to 2000
//'''scale a PWM value''' # default to servo range of 1000 to 2000
if (MainV2.comPort.param.Count > 0)
{
min_pwm = int.Parse(MainV2.comPort.param["RC3_MIN"].ToString());
max_pwm = int.Parse(MainV2.comPort.param["RC3_MAX"].ToString());
}
float p = (servo - min_pwm) / (max_pwm - min_pwm);
float p = (servo - min_pwm) / (max_pwm - min_pwm);
float v = min + p * (max - min);
float v = min + p * (max - min);
if (v < min)
v = min ;
if (v > max)
v = max ;
return v;
}
if (v < min)
v = min;
if (v > max)
v = max;
return v;
}
public void update(ref double[] servos, ArdupilotMega.GCSViews.Simulation.FGNetFDM fdm)
{
@ -75,7 +75,7 @@ namespace ArdupilotMega.HIL
}
else
{
motor_speed[i] = scale_rc(i,(float)servos[i], 0.0f, 1.0f);
motor_speed[i] = scale_rc(i, (float)servos[i], 0.0f, 1.0f);
//servos[i] = motor_speed[i];
}
}
@ -90,12 +90,12 @@ namespace ArdupilotMega.HIL
yaw_rate = 0;
*/
// Console.WriteLine("\nin m {0:0.000000} {1:0.000000} {2:0.000000} {3:0.000000}", m[0], m[1], m[2], m[3]);
// Console.WriteLine("in vel {0:0.000000} {1:0.000000} {2:0.000000}", velocity.X, velocity.Y, velocity.Z);
// Console.WriteLine("\nin m {0:0.000000} {1:0.000000} {2:0.000000} {3:0.000000}", m[0], m[1], m[2], m[3]);
// Console.WriteLine("in vel {0:0.000000} {1:0.000000} {2:0.000000}", velocity.X, velocity.Y, velocity.Z);
//Console.WriteLine("in r {0:0.000000} p {1:0.000000} y {2:0.000000} - r {3:0.000000} p {4:0.000000} y {5:0.000000}", roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate);
// m[0] *= 0.5;
// m[0] *= 0.5;
//# how much time has passed?
DateTime t = DateTime.Now;
@ -112,21 +112,21 @@ namespace ArdupilotMega.HIL
double pitch_accel = (m[2] - m[3]) * 5000.0;
double yaw_accel = -((m[2] + m[3]) - (m[0] + m[1])) * 400.0;
// Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
// Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
//# update rotational rates
roll_rate += roll_accel * delta_time.TotalSeconds;
pitch_rate += pitch_accel * delta_time.TotalSeconds;
yaw_rate += yaw_accel * delta_time.TotalSeconds;
// Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
// Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
//# update rotation
roll += roll_rate * delta_time.TotalSeconds;
pitch += pitch_rate * delta_time.TotalSeconds;
yaw += yaw_rate * delta_time.TotalSeconds;
// Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
// Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
//Console.WriteLine("r {0:0.0} p {1:0.0} y {2:0.0} - r {3:0.0} p {4:0.0} y {5:0.0} ms {6:0.000}", roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate, delta_time.TotalSeconds);
@ -155,7 +155,7 @@ namespace ArdupilotMega.HIL
old_position = new Vector3d(position);
position += velocity * delta_time.TotalSeconds;
// Console.WriteLine(fdm.agl + " "+ fdm.altitude);
// Console.WriteLine(fdm.agl + " "+ fdm.altitude);
//Console.WriteLine("Z {0} halt {1} < gl {2} fh {3}" ,position.Z , home_altitude , ground_level , frame_height);
@ -176,7 +176,7 @@ namespace ArdupilotMega.HIL
{
if (old_position.Z + home_altitude > ground_level + frame_height)
{
// Console.WriteLine("Hit ground at {0} m/s", (-velocity.Z));
// Console.WriteLine("Hit ground at {0} m/s", (-velocity.Z));
}
velocity = new Vector3d(0, 0, 0);
roll_rate = 0;
@ -186,14 +186,18 @@ namespace ArdupilotMega.HIL
pitch = 0;
position = new Vector3d(position.X, position.Y,
ground_level + frame_height - home_altitude + 0);
// Console.WriteLine("here " + position.Z);
// Console.WriteLine("here " + position.Z);
}
//# update lat/lon/altitude
update_position();
// send to apm
#if MAVLINK10
ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t();
#else
ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t();
#endif
ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();
@ -206,6 +210,19 @@ namespace ArdupilotMega.HIL
att.pitchspeed = (float)pitch_rate * deg2rad;
att.yawspeed = (float)yaw_rate * deg2rad;
#if MAVLINK10
gps.alt = ((int)(altitude * 1000));
gps.fix_type = 3;
gps.vel = (ushort)(Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)) * 100);
gps.cog = (ushort)((((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360) * 100);
gps.lat = (int)(latitude* 1.0e7);
gps.lon = (int)(longitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
asp.airspeed = gps.vel;
#else
gps.alt = ((float)(altitude));
gps.fix_type = 3;
gps.v = ((float)Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)));
@ -215,20 +232,21 @@ namespace ArdupilotMega.HIL
gps.usec = ((ulong)DateTime.Now.Ticks);
asp.airspeed = gps.v;
#endif
MainV2.comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_ATTITUDE, att);
MainV2.comPort.sendPacket(att);
MAVLink.__mavlink_raw_pressure_t pres = new MAVLink.__mavlink_raw_pressure_t();
double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * gps.alt, 5.25588));
pres.press_diff1 = (short)(int)(calc); // 0 alt is 0 pa
MainV2.comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_RAW_PRESSURE, pres);
MainV2.comPort.sendPacket(pres);
MainV2.comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_VFR_HUD, asp);
MainV2.comPort.sendPacket(asp);
if (framecount % 10 == 0)
{// 50 / 10 = 5 hz
MainV2.comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_GPS_RAW, gps);
MainV2.comPort.sendPacket(gps);
//Console.WriteLine(DateTime.Now.Millisecond + " GPS" );
}

14
Tools/ArdupilotMegaPlanner/Joystick.cs
View File

@ -326,20 +326,8 @@ namespace ArdupilotMega
{
try
{
MAVLink.__mavlink_set_nav_mode_t navmode = new MAVLink.__mavlink_set_nav_mode_t();
MainV2.comPort.setMode(but.mode);
MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t();
if (Common.translateMode(but.mode, ref navmode, ref mode))
{
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode);
System.Threading.Thread.Sleep(10);
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode);
System.Threading.Thread.Sleep(10);
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
System.Threading.Thread.Sleep(10);
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
}
}
catch { System.Windows.Forms.MessageBox.Show("Failed to change Modes"); }
});

6
Tools/ArdupilotMegaPlanner/JoystickSetup.cs
View File

@ -162,11 +162,11 @@ namespace ArdupilotMega
rc.chan7_raw = 0;
rc.chan8_raw = 0;
MainV2.comPort.generatePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc);
MainV2.comPort.sendPacket(rc);
System.Threading.Thread.Sleep(20);
MainV2.comPort.generatePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc);
MainV2.comPort.sendPacket(rc);
System.Threading.Thread.Sleep(20);
MainV2.comPort.generatePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc);
MainV2.comPort.sendPacket(rc);
//timer1.Stop();
MainV2.joystick.enabled = false;

488
Tools/ArdupilotMegaPlanner/MAVLink.cs
View File

@ -198,7 +198,7 @@ namespace ArdupilotMega
}
}
frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + " compid "+compid+") ";
frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + " compid " + compid + ") ";
frm.Refresh();
if (getparams == true)
getParamList();
@ -214,7 +214,7 @@ namespace ArdupilotMega
packetslost = 0;
}
public static byte[] StructureToByteArrayEndian(params object[] list)
byte[] StructureToByteArrayEndian(params object[] list)
{
// The copy is made becuase SetValue won't work on a struct.
// Boxing was used because SetValue works on classes/objects.
@ -346,12 +346,26 @@ namespace ArdupilotMega
}
}
public void sendPacket(object indata)
{
byte a = 0;
foreach (Type ty in mavstructs)
{
if (ty == indata.GetType())
{
generatePacket(a, indata);
return;
}
a++;
}
}
/// <summary>
/// Generate a Mavlink Packet and write to serial
/// </summary>
/// <param name="messageType">type number</param>
/// <param name="indata">struct of data</param>
public void generatePacket(byte messageType, object indata)
void generatePacket(byte messageType, object indata)
{
byte[] data;
@ -378,7 +392,11 @@ namespace ArdupilotMega
packet[1] = (byte)data.Length;
packet[2] = packetcount;
packet[3] = 255; // this is always 255 - MYGCS
packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_WAYPOINTPLANNER;
#if MAVLINK10
packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
#else
packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_WAYPOINTPLANNER;
#endif
packet[5] = messageType;
int i = 6;
@ -470,9 +488,11 @@ namespace ArdupilotMega
byte[] temp = ASCIIEncoding.ASCII.GetBytes(paramname);
modifyParamForDisplay(false, paramname, ref value);
Array.Resize(ref temp, 15);
#if MAVLINK10
Array.Resize(ref temp, 16);
#else
Array.Resize(ref temp, 15);
#endif
req.param_id = temp;
req.param_value = (value);
@ -523,7 +543,7 @@ namespace ArdupilotMega
if (st != paramname)
{
Console.WriteLine("MAVLINK bad param responce - {0} vs {1}",paramname,st);
Console.WriteLine("MAVLINK bad param responce - {0} vs {1}", paramname, st);
continue;
}
@ -539,12 +559,6 @@ namespace ArdupilotMega
}
}
public struct _param
{
public string name;
public float value;
}
/// <summary>
/// Get param list from apm
/// </summary>
@ -593,6 +607,7 @@ namespace ArdupilotMega
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_READ, rereq);
restart = DateTime.Now;
}
System.Windows.Forms.Application.DoEvents();
byte[] buffer = readPacket();
if (buffer.Length > 5)
@ -614,8 +629,6 @@ namespace ArdupilotMega
z = (par.param_count);
Console.WriteLine(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (z - 1));
if (nextid == (par.param_index))
{
nextid++;
@ -644,6 +657,8 @@ namespace ArdupilotMega
st = st.Substring(0, pos);
}
Console.WriteLine(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (z - 1) + " name: " + st);
modifyParamForDisplay(true, st, ref par.param_value);
param[st] = (par.param_value);
@ -668,7 +683,8 @@ namespace ArdupilotMega
|| paramname.ToUpper().EndsWith("XTRK_ANGLE_CD") || paramname.ToUpper().EndsWith("LIM_PITCH_MAX") || paramname.ToUpper().EndsWith("LIM_PITCH_MIN")
|| paramname.ToUpper().EndsWith("LIM_ROLL_CD") || paramname.ToUpper().EndsWith("PITCH_MAX") || paramname.ToUpper().EndsWith("WP_SPEED_MAX"))
{
if (paramname.ToUpper().EndsWith("THR_HOLD_IMAX")) {
if (paramname.ToUpper().EndsWith("THR_HOLD_IMAX"))
{
return;
}
@ -721,16 +737,138 @@ namespace ArdupilotMega
public void setWPACK()
{
#if MAVLINK10
MAVLink.__mavlink_mission_ack_t req = new MAVLink.__mavlink_mission_ack_t();
req.target_system = sysid;
req.target_component = compid;
req.type = 0;
generatePacket(MAVLINK_MSG_ID_MISSION_ACK, req);
#else
MAVLink.__mavlink_waypoint_ack_t req = new MAVLink.__mavlink_waypoint_ack_t();
req.target_system = sysid;
req.target_component = compid;
req.type = 0;
generatePacket(MAVLINK_MSG_ID_WAYPOINT_ACK, req);
#endif
}
public bool setWPCurrent(ushort index)
{
#if MAVLINK10
MainV2.givecomport = true;
byte[] buffer;
__mavlink_mission_set_current_t req = new __mavlink_mission_set_current_t();
req.target_system = sysid;
req.target_component = compid;
req.seq = index;
generatePacket(MAVLINK_MSG_ID_MISSION_SET_CURRENT, req);
DateTime start = DateTime.Now;
int retrys = 5;
while (true)
{
if (!(start.AddMilliseconds(2000) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("setWPCurrent Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_MISSION_SET_CURRENT, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - setWPCurrent");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_MISSION_CURRENT)
{
MainV2.givecomport = false;
return true;
}
}
}
}
public bool doCommand(MAV_CMD actionid)
{
MainV2.givecomport = true;
byte[] buffer;
__mavlink_command_long_t req = new __mavlink_command_long_t();
req.target_system = sysid;
req.target_component = compid;
req.command = (ushort)actionid;
generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req);
DateTime start = DateTime.Now;
int retrys = 3;
int timeout = 2000;
// imu calib take a little while
if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION)
{
retrys = 1;
timeout = 6000;
}
while (true)
{
if (!(start.AddMilliseconds(timeout) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("doAction Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - doAction");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_COMMAND_ACK)
{
__mavlink_command_ack_t ack = new __mavlink_command_ack_t();
object temp = (object)ack;
ByteArrayToStructure(buffer, ref temp, 6);
ack = (__mavlink_command_ack_t)(temp);
if (ack.result == (byte)MAV_RESULT.MAV_RESULT_ACCEPTED)
{
MainV2.givecomport = false;
return true;
}
else
{
MainV2.givecomport = false;
return false;
}
}
}
}
#else
MainV2.givecomport = true;
byte[] buffer;
@ -837,6 +975,8 @@ namespace ArdupilotMega
}
}
}
#endif
}
public void requestDatastream(byte id, byte hzrate)
@ -983,6 +1123,60 @@ namespace ArdupilotMega
{
MainV2.givecomport = true;
byte[] buffer;
#if MAVLINK10
__mavlink_mission_request_list_t req = new __mavlink_mission_request_list_t();
req.target_system = sysid;
req.target_component = compid;
// request list
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req);
DateTime start = DateTime.Now;
int retrys = 6;
while (true)
{
if (!(start.AddMilliseconds(500) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("getWPCount Retry " + retrys + " - giv com " + MainV2.givecomport);
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
//return (byte)int.Parse(param["WP_TOTAL"].ToString());
throw new Exception("Timeout on read - getWPCount");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_MISSION_COUNT)
{
__mavlink_mission_count_t count = new __mavlink_mission_count_t();
object temp = (object)count;
ByteArrayToStructure(buffer, ref temp, 6);
count = (__mavlink_mission_count_t)(temp);
Console.WriteLine("wpcount: " + count.count);
MainV2.givecomport = false;
return (byte)count.count; // should be ushort, but apm has limited wp count < byte
}
else
{
Console.WriteLine(DateTime.Now + " PC wpcount " + buffer[5] + " need " + MAVLINK_MSG_ID_MISSION_COUNT + " " + this.BaseStream.BytesToRead);
}
}
}
#else
__mavlink_waypoint_request_list_t req = new __mavlink_waypoint_request_list_t();
@ -1028,6 +1222,8 @@ namespace ArdupilotMega
}
}
}
#endif
}
/// <summary>
/// Gets specfied WP
@ -1038,6 +1234,56 @@ namespace ArdupilotMega
{
MainV2.givecomport = true;
Locationwp loc = new Locationwp();
#if MAVLINK10
__mavlink_mission_request_t req = new __mavlink_mission_request_t();
req.target_system = sysid;
req.target_component = compid;
req.seq = index;
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
// request
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST, req);
DateTime start = DateTime.Now;
int retrys = 5;
while (true)
{
if (!(start.AddMilliseconds(800) > DateTime.Now)) // apm times out after 1000ms
{
if (retrys > 0)
{
Console.WriteLine("getWP Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - getWP");
}
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
byte[] buffer = readPacket();
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_MISSION_ITEM)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
__mavlink_mission_item_t wp = new __mavlink_mission_item_t();
object temp = (object)wp;
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
ByteArrayToStructure(buffer, ref temp, 6);
wp = (__mavlink_mission_item_t)(temp);
#else
__mavlink_waypoint_request_t req = new __mavlink_waypoint_request_t();
@ -1087,6 +1333,8 @@ namespace ArdupilotMega
wp = (__mavlink_waypoint_t)(temp);
#endif
loc.options = (byte)(wp.frame & 0x1);
loc.id = (byte)(wp.command);
loc.p1 = (byte)(wp.param1);
@ -1108,7 +1356,7 @@ namespace ArdupilotMega
case (byte)MAV_CMD.LOITER_TURNS:
case (byte)MAV_CMD.TAKEOFF:
case (byte)MAV_CMD.DO_SET_HOME:
case (byte)MAV_CMD.DO_SET_ROI:
//case (byte)MAV_CMD.DO_SET_ROI:
loc.alt = (int)((wp.z) * 100);
loc.lat = (int)((wp.x) * 10000000);
loc.lng = (int)((wp.y) * 10000000);
@ -1254,6 +1502,65 @@ namespace ArdupilotMega
/// <param name="wp_total"></param>
public void setWPTotal(ushort wp_total)
{
#if MAVLINK10
MainV2.givecomport = true;
__mavlink_mission_count_t req = new __mavlink_mission_count_t();
req.target_system = sysid;
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_COUNT
req.count = wp_total;
generatePacket(MAVLINK_MSG_ID_MISSION_COUNT, req);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("setWPTotal Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_MISSION_COUNT, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - setWPTotal");
}
byte[] buffer = readPacket();
if (buffer.Length > 9)
{
if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST)
{
__mavlink_mission_request_t request = new __mavlink_mission_request_t();
object temp = (object)request;
ByteArrayToStructure(buffer, ref temp, 6);
request = (__mavlink_mission_request_t)(temp);
if (request.seq == 0)
{
if (param["WP_TOTAL"] != null)
param["WP_TOTAL"] = (float)wp_total - 1;
if (param["CMD_TOTAL"] != null)
param["CMD_TOTAL"] = (float)wp_total - 1;
MainV2.givecomport = false;
return;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
}
}
}
#else
MainV2.givecomport = true;
__mavlink_waypoint_count_t req = new __mavlink_waypoint_count_t();
@ -1285,12 +1592,25 @@ namespace ArdupilotMega
byte[] buffer = readPacket();
if (buffer.Length > 9)
{
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST && buffer[9] == 0)
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST)
{
param["WP_TOTAL"] = (float)wp_total - 1;
param["CMD_TOTAL"] = (float)wp_total - 1;
MainV2.givecomport = false;
return;
__mavlink_waypoint_request_t request = new __mavlink_waypoint_request_t();
object temp = (object)request;
ByteArrayToStructure(buffer, ref temp, 6);
request = (__mavlink_waypoint_request_t)(temp);
if (request.seq == 0)
{
if (param["WP_TOTAL"] != null)
param["WP_TOTAL"] = (float)wp_total - 1;
if (param["CMD_TOTAL"] != null)
param["CMD_TOTAL"] = (float)wp_total - 1;
MainV2.givecomport = false;
return;
}
}
else
{
@ -1298,6 +1618,8 @@ namespace ArdupilotMega
}
}
}
#endif
}
/// <summary>
@ -1310,10 +1632,14 @@ namespace ArdupilotMega
public void setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current)
{
MainV2.givecomport = true;
__mavlink_waypoint_t req = new __mavlink_waypoint_t();
#if MAVLINK10
__mavlink_mission_item_t req = new __mavlink_mission_item_t();
#else
__mavlink_waypoint_t req = new __mavlink_waypoint_t();
#endif
req.target_system = sysid;
req.target_component = compid; // MAVLINK_MSG_ID_WAYPOINT
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM
req.command = loc.id;
req.param1 = loc.p1;
@ -1390,7 +1716,11 @@ namespace ArdupilotMega
Console.WriteLine("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index);
// request
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
#if MAVLINK10
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
#else
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
#endif
DateTime start = DateTime.Now;
int retrys = 6;
@ -1402,7 +1732,11 @@ namespace ArdupilotMega
if (retrys > 0)
{
Console.WriteLine("setWP Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
#if MAVLINK10
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
#else
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
#endif
start = DateTime.Now;
retrys--;
continue;
@ -1413,7 +1747,48 @@ namespace ArdupilotMega
byte[] buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_ACK)
#if MAVLINK10
if (buffer[5] == MAVLINK_MSG_ID_MISSION_ACK)
{
__mavlink_mission_ack_t ans = new __mavlink_mission_ack_t();
object temp = (object)ans;
ByteArrayToStructure(buffer, ref temp, 6);
ans = (__mavlink_mission_ack_t)(temp);
Console.WriteLine("set wp " + index + " ACK 47 : " + buffer[5] + " ans " + Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString()));
break;
}
else if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST)
{
__mavlink_mission_request_t ans = new __mavlink_mission_request_t();
object temp = (object)ans;
ByteArrayToStructure(buffer, ref temp, 6);
ans = (__mavlink_mission_request_t)(temp);
if (ans.seq == (index + 1))
{
Console.WriteLine("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]);
MainV2.givecomport = false;
break;
}
else
{
Console.WriteLine("set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + buffer[5] + " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component);
//break;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
}
#else
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_ACK)
{ //__mavlink_waypoint_request_t
Console.WriteLine("set wp " + index + " ACK 47 : " + buffer[5]);
break;
@ -1446,11 +1821,12 @@ namespace ArdupilotMega
{
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
}
#endif
}
}
}
public void setMountConfigure(MAV_MOUNT_MODE mountmode,bool stabroll,bool stabpitch,bool stabyaw)
public void setMountConfigure(MAV_MOUNT_MODE mountmode, bool stabroll, bool stabpitch, bool stabyaw)
{
__mavlink_mount_configure_t req = new __mavlink_mount_configure_t();
@ -1466,7 +1842,7 @@ namespace ArdupilotMega
generatePacket(MAVLINK_MSG_ID_MOUNT_CONFIGURE, req);
}
public void setMountControl(double pa,double pb,double pc,bool islatlng)
public void setMountControl(double pa, double pb, double pc, bool islatlng)
{
__mavlink_mount_control_t req = new __mavlink_mount_control_t();
@ -1492,6 +1868,20 @@ namespace ArdupilotMega
public void setMode(string modein)
{
#if MAVLINK10
try
{
MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t();
if (Common.translateMode(modein, ref mode))
{
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
System.Threading.Thread.Sleep(10);
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
}
}
catch { System.Windows.Forms.MessageBox.Show("Failed to change Modes"); }
#else
try
{
MAVLink.__mavlink_set_nav_mode_t navmode = new MAVLink.__mavlink_set_nav_mode_t();
@ -1510,6 +1900,8 @@ namespace ArdupilotMega
}
}
catch { System.Windows.Forms.MessageBox.Show("Failed to change Modes"); }
#endif
}
/// <summary>
@ -1661,7 +2053,7 @@ namespace ArdupilotMega
if (bpstime.Second != DateTime.Now.Second && !logreadmode)
{
Console.WriteLine("bps {0} loss {1} left {2} mem {3}", bps1, synclost, BaseStream.BytesToRead,System.GC.GetTotalMemory(false));
Console.WriteLine("bps {0} loss {1} left {2} mem {3}", bps1, synclost, BaseStream.BytesToRead, System.GC.GetTotalMemory(false));
bps2 = bps1; // prev sec
bps1 = 0; // current sec
bpstime = DateTime.Now;
@ -1683,6 +2075,12 @@ namespace ArdupilotMega
crc = crc_accumulate(MAVLINK_MESSAGE_CRCS[temp[5]], crc);
}
if (temp.Length > 5 && temp[1] != MAVLINK_MESSAGE_LENGTHS[temp[5]])
{
Console.WriteLine("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", temp.Length, temp[5]);
return new byte[0];
}
if (temp.Length < 5 || temp[temp.Length - 1] != (crc >> 8) || temp[temp.Length - 2] != (crc & 0xff))
{
int packetno = 0;
@ -1743,12 +2141,31 @@ namespace ArdupilotMega
if (temp[5] == MAVLink.MAVLINK_MSG_ID_STATUSTEXT) // status text
{
string logdata = DateTime.Now.Millisecond + " " + Encoding.ASCII.GetString(temp, 6, temp.Length - 6);
string logdata = DateTime.Now + " " + Encoding.ASCII.GetString(temp, 6, temp.Length - 6);
int ind = logdata.IndexOf('\0');
if (ind != -1)
logdata = logdata.Substring(0, ind);
Console.WriteLine(logdata);
}
#if MAVLINK10
if (temp[5] == MAVLINK_MSG_ID_MISSION_COUNT)
{
// clear old
wps = new PointLatLngAlt[wps.Length];
}
if (temp[5] == MAVLink.MAVLINK_MSG_ID_MISSION_ITEM)
{
__mavlink_mission_item_t wp = new __mavlink_mission_item_t();
object structtemp = (object)wp;
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
ByteArrayToStructure(temp, ref structtemp, 6);
wp = (__mavlink_mission_item_t)(structtemp);
#else
if (temp[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT)
{
@ -1768,7 +2185,8 @@ namespace ArdupilotMega
wp = (__mavlink_waypoint_t)(structtemp);
wps[wp.seq] = new PointLatLngAlt(wp.x,wp.y,wp.z,wp.seq.ToString());
#endif
wps[wp.seq] = new PointLatLngAlt(wp.x, wp.y, wp.z, wp.seq.ToString());
}
try
@ -1873,7 +2291,7 @@ namespace ArdupilotMega
while (a < length)
{
temp[a] = (byte)logplaybackfile.ReadByte();
if (temp[0] != 'U')
if (temp[0] != 'U' && temp[0] != 254)
{
Console.WriteLine("lost sync byte {0} pos {1}", temp[0], logplaybackfile.BaseStream.Position);
a = 0;
@ -1927,7 +2345,7 @@ namespace ArdupilotMega
}
public static byte[] StructureToByteArray(object obj)
byte[] StructureToByteArray(object obj)
{
int len = Marshal.SizeOf(obj);

1556
Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs
View File

File diff suppressed because it is too large Load Diff

1494
Tools/ArdupilotMegaPlanner/MAVLinkTypes0.9.cs Normal file
View File

File diff suppressed because it is too large Load Diff

81
Tools/ArdupilotMegaPlanner/MainV2.cs
View File

@ -58,6 +58,13 @@ namespace ArdupilotMega
public static List<System.Threading.Thread> threads = new List<System.Threading.Thread>();
public static MainV2 instance = null;
/*
* "PITCH_KP",
"PITCH_KI",
"PITCH_LIM",
*/
public enum Firmwares
{
ArduPlane,
@ -468,7 +475,6 @@ namespace ArdupilotMega
private void MenuConfiguration_Click(object sender, EventArgs e)
{
MyView.SuspendLayout();
MyView.Controls.Clear();
GCSViews.Terminal.threadrun = false;
@ -499,11 +505,11 @@ namespace ArdupilotMega
temp.BackColor = Color.FromArgb(0x26, 0x27, 0x28);
temp.ResumeLayout();
temp.Size = MyView.Size;
//temp.Parent = MyView;
MyView.Controls.Add(temp);
MyView.ResumeLayout();
}
private void MenuSimulation_Click(object sender, EventArgs e)
@ -619,14 +625,14 @@ namespace ArdupilotMega
comPort.BaseStream = new TcpSerial();
}
else
if (CMB_serialport.Text == "UDP")
{
comPort.BaseStream = new UdpSerial();
}
else
{
comPort.BaseStream = new SerialPort();
}
if (CMB_serialport.Text == "UDP")
{
comPort.BaseStream = new UdpSerial();
}
else
{
comPort.BaseStream = new SerialPort();
}
try
{
comPort.BaseStream.BaudRate = int.Parse(CMB_baudrate.Text);
@ -684,10 +690,13 @@ namespace ArdupilotMega
this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.disconnect;
}
catch (Exception ex) {
try {
catch (Exception ex)
{
try
{
comPort.Close();
} catch { }
}
catch { }
try
{
string version = ArduinoDetect.DetectVersion(comPort.BaseStream.PortName);
@ -940,7 +949,7 @@ namespace ArdupilotMega
if (lastjoystick.AddMilliseconds(50) < DateTime.Now)
{
// Console.WriteLine(DateTime.Now.Millisecond + " {0} {1} {2} {3} ", rc.chan1_raw, rc.chan2_raw, rc.chan3_raw, rc.chan4_raw);
comPort.generatePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc);
comPort.sendPacket(rc);
lastjoystick = DateTime.Now;
}
@ -1064,18 +1073,25 @@ namespace ArdupilotMega
MAVLink.__mavlink_heartbeat_t htb = new MAVLink.__mavlink_heartbeat_t();
#if MAVLINK10
htb.type = (byte)MAVLink.MAV_TYPE.MAV_TYPE_GCS;
htb.autopilot = (byte)MAVLink.MAV_AUTOPILOT.MAV_AUTOPILOT_ARDUPILOTMEGA;
htb.mavlink_version = 3;
#else
htb.type = (byte)MAVLink.MAV_TYPE.MAV_GENERIC;
htb.autopilot = (byte)MAVLink.MAV_AUTOPILOT_TYPE.MAV_AUTOPILOT_ARDUPILOTMEGA;
htb.mavlink_version = 2;
#endif
comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_HEARTBEAT, htb);
comPort.sendPacket(htb);
heatbeatsend = DateTime.Now;
}
// data loss warning
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10)
{
if (speechenable && talk != null) {
if (speechenable && talk != null)
{
if (MainV2.talk.State == SynthesizerState.Ready)
MainV2.talk.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds");
}
@ -1086,7 +1102,8 @@ namespace ArdupilotMega
while (comPort.BaseStream.BytesToRead > minbytes && givecomport == false)
comPort.readPacket();
}
catch (Exception e) {
catch (Exception e)
{
Console.WriteLine("Serial Reader fail :" + e.Message);
try
{
@ -1195,7 +1212,7 @@ namespace ArdupilotMega
{
listener.Start();
}
catch { Console.WriteLine("do you have the planner open already");return; } // in use
catch { Console.WriteLine("do you have the planner open already"); return; } // in use
// Enter the listening loop.
while (true)
{
@ -1260,19 +1277,22 @@ namespace ArdupilotMega
//message
stream.WriteByte(0xff);
}
} else if (url.Contains(".html")) {
BinaryReader file = new BinaryReader(File.Open("hud.html",FileMode.Open,FileAccess.Read,FileShare.Read));
}
else if (url.Contains(".html"))
{
BinaryReader file = new BinaryReader(File.Open("hud.html", FileMode.Open, FileAccess.Read, FileShare.Read));
byte[] buffer = new byte[1024];
while (file.PeekChar() != -1) {
while (file.PeekChar() != -1)
{
int leng = file.Read(buffer,0,buffer.Length);
int leng = file.Read(buffer, 0, buffer.Length);
stream.Write(buffer,0,leng);
stream.Write(buffer, 0, leng);
}
file.Close();
stream.Close();
}
else if (url.ToLower().Contains("hud") || url.ToLower().Contains("map"))
else if (url.ToLower().Contains("hud.jpg") || url.ToLower().Contains("map.jpg") || url.ToLower().Contains("both.jpg"))
{
string header = "HTTP/1.1 200 OK\r\nContent-Type: multipart/x-mixed-replace;boundary=APMPLANNER\n\n--APMPLANNER\r\n";
byte[] temp = encoding.GetBytes(header);
@ -1496,7 +1516,7 @@ namespace ArdupilotMega
if (fi.Length != response.ContentLength) // && response.Headers[HttpResponseHeader.ETag] != "0")
{
StreamWriter sw = new StreamWriter(path+".etag");
StreamWriter sw = new StreamWriter(path + ".etag");
sw.WriteLine(response.Headers[HttpResponseHeader.ETag]);
sw.Close();
getfile = true;
@ -1622,7 +1642,12 @@ namespace ArdupilotMega
}
if (keyData == (Keys.Control | Keys.Y)) // for ryan beall
{
MainV2.comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_STORAGE_WRITE);
#if MAVLINK10
int fixme;
//MainV2.comPort.doCommand(MAVLink.MAV_ACTION.MAV_ACTION_STORAGE_WRITE);
#else
MainV2.comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_STORAGE_WRITE);
#endif
MessageBox.Show("Done MAV_ACTION_STORAGE_WRITE");
return true;
}

7
Tools/ArdupilotMegaPlanner/Program.cs
View File

@ -22,12 +22,19 @@ namespace ArdupilotMega
Application.ThreadException += new System.Threading.ThreadExceptionEventHandler(Application_ThreadException);
Application.Idle += new EventHandler(Application_Idle);
Application.EnableVisualStyles();
Application.SetCompatibleTextRenderingDefault(false);
//Application.Run(new Camera());
Application.Run(new MainV2());
}
static void Application_Idle(object sender, EventArgs e)
{
//Console.WriteLine("Idle");
}
static void Application_ThreadException(object sender, System.Threading.ThreadExceptionEventArgs e)
{
Exception ex = e.Exception;

2
Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
View File

@ -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.89")]
[assembly: AssemblyFileVersion("1.0.90")]
[assembly: NeutralResourcesLanguageAttribute("")]

27
Tools/ArdupilotMegaPlanner/Setup/Setup.cs
View File

@ -221,7 +221,7 @@ namespace ArdupilotMega.Setup
}
catch { MessageBox.Show("Failed to set Channel " + (a + 1).ToString()); }
data = data +"CH" + (a+1) + " " + rcmin[a] + " | " + rcmax[a] + "\n";
data = data + "CH" + (a + 1) + " " + rcmin[a] + " | " + rcmax[a] + "\n";
}
MainV2.cs.raterc = oldrc;
@ -470,7 +470,7 @@ namespace ArdupilotMega.Setup
MainV2.comPort.setParam("FLTMODE5", (float)(int)Enum.Parse(typeof(Common.ac2modes), CMB_fmode5.Text));
MainV2.comPort.setParam("FLTMODE6", (float)(int)Enum.Parse(typeof(Common.ac2modes), CMB_fmode6.Text));
float value = (float)(CB_simple1.Checked ? 1 : 0) + (CB_simple2.Checked ? 1 << 1 : 0) + (CB_simple3.Checked ? 1 <<2 : 0)
float value = (float)(CB_simple1.Checked ? 1 : 0) + (CB_simple2.Checked ? 1 << 1 : 0) + (CB_simple3.Checked ? 1 << 2 : 0)
+ (CB_simple4.Checked ? 1 << 3 : 0) + (CB_simple5.Checked ? 1 << 4 : 0) + (CB_simple6.Checked ? 1 << 5 : 0);
if (MainV2.comPort.param.ContainsKey("SIMPLE"))
MainV2.comPort.setParam("SIMPLE", value);
@ -570,7 +570,7 @@ namespace ArdupilotMega.Setup
{
if (MainV2.comPort.param["ARSPD_ENABLE"] == null)
{
MessageBox.Show("Not Available on "+ MainV2.cs.firmware.ToString());
MessageBox.Show("Not Available on " + MainV2.cs.firmware.ToString());
}
else
{
@ -646,7 +646,7 @@ namespace ArdupilotMega.Setup
if (startup || ((TextBox)sender).Enabled == false)
return;
try
{
{
if (MainV2.comPort.param["INPUT_VOLTS"] == null)
{
MessageBox.Show("Not Available");
@ -1022,14 +1022,19 @@ namespace ArdupilotMega.Setup
MainV2.comPort.setParam("HS4_MIN", HS4.minline);
MainV2.comPort.setParam("HS4_MAX", HS4.maxline);
}
catch { MessageBox.Show("Failed to set min/max"); }
catch { MessageBox.Show("Failed to set min/max"); }
}
private void BUT_levelac2_Click(object sender, EventArgs e)
{
try
{
#if MAVLINK10
int fixme;
// MainV2.comPort.doCommand(MAVLink.MAV_ACTION.MAV_ACTION_CALIBRATE_ACC);
#else
MainV2.comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_CALIBRATE_ACC);
#endif
BUT_levelac2.Text = "Complete";
}
@ -1049,15 +1054,17 @@ namespace ArdupilotMega.Setup
catch { MessageBox.Show("Webpage open failed... do you have a virus?\nhttp://www.magnetic-declination.com/"); }
}
void reverseChannel(string name,bool normalreverse,Control progressbar)
void reverseChannel(string name, bool normalreverse, Control progressbar)
{
if (normalreverse == true)
{
((HorizontalProgressBar2)progressbar).reverse = true;
((HorizontalProgressBar2)progressbar).BackgroundColor = Color.FromArgb(148, 193, 31);
((HorizontalProgressBar2)progressbar).ValueColor = Color.FromArgb(0x43, 0x44, 0x45);
}
else
{
((HorizontalProgressBar2)progressbar).reverse = false;
((HorizontalProgressBar2)progressbar).BackgroundColor = Color.FromArgb(0x43, 0x44, 0x45);
((HorizontalProgressBar2)progressbar).ValueColor = Color.FromArgb(148, 193, 31);
}
@ -1083,22 +1090,22 @@ namespace ArdupilotMega.Setup
private void CHK_revch1_CheckedChanged(object sender, EventArgs e)
{
reverseChannel("RC1_REV", ((CheckBox)sender).Checked,BARroll);
reverseChannel("RC1_REV", ((CheckBox)sender).Checked, BARroll);
}
private void CHK_revch2_CheckedChanged(object sender, EventArgs e)
{
reverseChannel("RC2_REV", ((CheckBox)sender).Checked,BARpitch);
reverseChannel("RC2_REV", ((CheckBox)sender).Checked, BARpitch);
}
private void CHK_revch3_CheckedChanged(object sender, EventArgs e)
{
reverseChannel("RC3_REV", ((CheckBox)sender).Checked,BARthrottle);
reverseChannel("RC3_REV", ((CheckBox)sender).Checked, BARthrottle);
}
private void CHK_revch4_CheckedChanged(object sender, EventArgs e)
{
reverseChannel("RC4_REV", ((CheckBox)sender).Checked,BARyaw);
reverseChannel("RC4_REV", ((CheckBox)sender).Checked, BARyaw);
}
}
}

2
Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application
View File

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

30
Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml
View File

@ -36,23 +36,23 @@
<F1>Yaw Sensor</F1>
<F2>Nav Yaw</F2>
<F3>Yaw Error</F3>
<F4>Throttle Out</F4>
<F5>Sonar Alt</F5>
<F6>Baro Alt</F6>
<F7>Next WP Alt</F7>
<F8>Throttle I</F8>
<F4>Sonar Alt</F4>
<F5>Baro Alt</F5>
<F6>Next WP Alt</F6>
<F7>Nav Throttle</F7>
<F8>Angle boost</F8>
<F9>Manual boost</F9>
<F10>rc3 servo out</F10>
<F11>alt hold int</F11>
<F12>thro int</F12>
</CTUN>
<PM>
<F1>loop time</F1>
<F2>Main count</F2>
<F3>G_Dt_max</F3>
<F4>Gyro Sat</F4>
<F5>adc constr</F5>
<F6>renorm_sqrt</F6>
<F7>renorm_blowup</F7>
<F8>gps_fix count</F8>
<F9>imu_health</F9>
<F10>Throttle Sum</F10>
<F1>control mode</F1>
<F2>yaw mode</F2>
<F3>r p mode</F3>
<F4>thro mode</F4>
<F5>thro cruise</F5>
<F6>thro int</F6>
</PM>
<RAW>
<F1>Gyro X</F1>

30
Tools/ArdupilotMegaPlanner/dataflashlog.xml
View File

@ -36,23 +36,23 @@
<F1>Yaw Sensor</F1>
<F2>Nav Yaw</F2>
<F3>Yaw Error</F3>
<F4>Throttle Out</F4>
<F5>Sonar Alt</F5>
<F6>Baro Alt</F6>
<F7>Next WP Alt</F7>
<F8>Throttle I</F8>
<F4>Sonar Alt</F4>
<F5>Baro Alt</F5>
<F6>Next WP Alt</F6>
<F7>Nav Throttle</F7>
<F8>Angle boost</F8>
<F9>Manual boost</F9>
<F10>rc3 servo out</F10>
<F11>alt hold int</F11>
<F12>thro int</F12>
</CTUN>
<PM>
<F1>loop time</F1>
<F2>Main count</F2>
<F3>G_Dt_max</F3>
<F4>Gyro Sat</F4>
<F5>adc constr</F5>
<F6>renorm_sqrt</F6>
<F7>renorm_blowup</F7>
<F8>gps_fix count</F8>
<F9>imu_health</F9>
<F10>Throttle Sum</F10>
<F1>control mode</F1>
<F2>yaw mode</F2>
<F3>r p mode</F3>
<F4>thro mode</F4>
<F5>thro cruise</F5>
<F6>thro int</F6>
</PM>
<RAW>
<F1>Gyro X</F1>

13
Tools/ArdupilotMegaPlanner/mavlinklist.pl
View File

@ -2,8 +2,8 @@ $dir = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/common/";
$dir2 = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/ardupilotmega/";
# mavlink 1.0 with old structs
$dir = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include/common/";
$dir2 = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include/ardupilotmega/";
$dir = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include_v1.0/common/";
$dir2 = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/";
opendir(DIR,$dir) || die print $!;
@files2 = readdir(DIR);
@ -59,8 +59,12 @@ foreach $file (@files) {
}
if ($line =~ /#define (MAVLINK_MSG_ID[^\s]+)\s+([0-9]+)/) {
print OUT "\t\tpublic const byte ".$1 . " = " . $2 . ";\n";
$no = $2;
if ($line =~ /MAVLINK_MSG_ID_([0-9]+)_LEN/) {
next;
} else {
print OUT "\t\tpublic const byte ".$1 . " = " . $2 . ";\n";
$no = $2;
}
}
if ($line =~ /typedef struct(.*)/) {
if ($1 =~ /__mavlink_system|param_union/) {
@ -81,6 +85,7 @@ foreach $file (@files) {
$line =~ s/typedef/public/;
$line =~ s/uint8_t/public byte/;
$line =~ s/int8_t/public byte/;
$line =~ s/char/public byte/;
$line =~ s/^\s+float/public float/;
$line =~ s/uint16_t/public ushort/;
$line =~ s/uint32_t/public uint/;