APM Planner 1.0.92

update wp file format - QGC 110
fix old wp file format abs/rel issue
This commit is contained in:
Michael Oborne 2011-11-15 21:50:12 +08:00
parent c6c6a98b0b
commit 826c998871
6 changed files with 161 additions and 34 deletions

View File

@ -211,7 +211,6 @@ namespace ArdupilotMega
dowindcalc();
}
// Console.WriteLine("Updating CurrentState " + DateTime.Now.Millisecond);
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text
{

View File

@ -778,7 +778,16 @@ namespace ArdupilotMega.GCSViews
return;
}
string alt = (100 * MainV2.cs.multiplierdist).ToString("0");
string alt = "100";
if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2)
{
alt = (10 * MainV2.cs.multiplierdist).ToString("0");
}
else
{
alt = (100 * MainV2.cs.multiplierdist).ToString("0");
}
if (DialogResult.Cancel == Common.InputBox("Enter Alt", "Enter Guided Mode Alt", ref alt))
return;
@ -816,14 +825,18 @@ namespace ArdupilotMega.GCSViews
private void Zoomlevel_ValueChanged(object sender, EventArgs e)
{
if (gMapControl1.MaxZoom + 1 == (double)Zoomlevel.Value)
try
{
gMapControl1.Zoom = (double)Zoomlevel.Value - .1;
}
else
{
gMapControl1.Zoom = (double)Zoomlevel.Value;
if (gMapControl1.MaxZoom + 1 == (double)Zoomlevel.Value)
{
gMapControl1.Zoom = (double)Zoomlevel.Value - .1;
}
else
{
gMapControl1.Zoom = (double)Zoomlevel.Value;
}
}
catch { }
}
private void gMapControl1_MouseMove(object sender, MouseEventArgs e)

View File

@ -154,6 +154,7 @@ namespace ArdupilotMega.GCSViews
for (int i = 0; i < matchs.Count; i++)
{
Locationwp temp = new Locationwp();
temp.options = 1;
temp.id = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), matchs[i].Groups[1].Value.ToString().Replace("NAV_", ""), false);
temp.p1 = byte.Parse(matchs[i].Groups[2].Value.ToString());
@ -1019,8 +1020,8 @@ namespace ArdupilotMega.GCSViews
private void savewaypoints()
{
SaveFileDialog fd = new SaveFileDialog();
fd.Filter = "Ardupilot Mission (*.h)|*.*";
fd.DefaultExt = ".h";
fd.Filter = "Ardupilot Mission (*.txt)|*.*";
fd.DefaultExt = ".txt";
DialogResult result = fd.ShowDialog();
string file = fd.FileName;
if (file != "")
@ -1028,26 +1029,39 @@ namespace ArdupilotMega.GCSViews
try
{
StreamWriter sw = new StreamWriter(file);
sw.WriteLine("#define WP_RADIUS " + TXT_WPRad.Text.ToString() + " // What is the minimum distance to reach a waypoint?");
sw.WriteLine("#define LOITER_RADIUS " + TXT_loiterrad.Text.ToString() + " // How close to Loiter?");
sw.WriteLine("#define HOLD_CURRENT_ALT 0 // 1 = hold the current altitude, 0 = use the defined altitude to for RTL");
sw.WriteLine("#define ALT_TO_HOLD " + TXT_DefaultAlt.Text);
sw.WriteLine("");
sw.WriteLine("float mission[][5] = {");
sw.WriteLine("QGC WPL 110");
try
{
sw.WriteLine("0\t1\t3\t0\t0\t0\t0\t0\t" + double.Parse(TXT_homelat.Text).ToString("0.000000", new System.Globalization.CultureInfo("en-US")) + "\t" + double.Parse(TXT_homelng.Text).ToString("0.000000", new System.Globalization.CultureInfo("en-US")) + "\t" + double.Parse(TXT_homealt.Text).ToString("0.000000", new System.Globalization.CultureInfo("en-US")) + "\t1");
}
catch
{
sw.WriteLine("0\t1\t3\t0\t0\t0\t0\t0\t0\t0\t0\t1");
}
for (int a = 0; a < Commands.Rows.Count - 0; a++)
{
sw.Write("{" + Commands.Rows[a].Cells[0].Value.ToString() + ",");
sw.Write(Commands.Rows[a].Cells[1].Value.ToString() + ",");
sw.Write(double.Parse(Commands.Rows[a].Cells[2].Value.ToString()).ToString(new System.Globalization.CultureInfo("en-US")) + ",");
sw.Write(double.Parse(Commands.Rows[a].Cells[3].Value.ToString()).ToString(new System.Globalization.CultureInfo("en-US")) + ",");
sw.Write(double.Parse(Commands.Rows[a].Cells[4].Value.ToString()).ToString(new System.Globalization.CultureInfo("en-US")) + "}");
//if (a < Commands.Rows.Count - 2)
//{
sw.Write(",");
//}
byte mode = (byte)(MAVLink.MAV_CMD)Enum.Parse(typeof(MAVLink.MAV_CMD), Commands.Rows[a].Cells[0].Value.ToString());
sw.Write((a+1)); // seq
sw.Write("\t" +0); // current
sw.Write("\t" + (CHK_altmode.Checked == true ? (byte)MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL : (byte)MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT)); //frame
sw.Write("\t" + mode);
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[1].Value.ToString()).ToString("0.000000",new System.Globalization.CultureInfo("en-US")));
if (mode >= (byte)MAVLink.MAV_CMD.LAST) {
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[2].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[3].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[4].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
} else {
sw.Write("\t" + "0.000000");
sw.Write("\t" + "0.000000");
sw.Write("\t" + "0.000000");
}
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[3].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[4].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
sw.Write("\t" + double.Parse(Commands.Rows[a].Cells[2].Value.ToString()).ToString("0.000000", new System.Globalization.CultureInfo("en-US")));
sw.Write("\t" + 1);
sw.WriteLine("");
}
sw.WriteLine("};");
sw.Close();
}
catch (Exception) { MessageBox.Show("Error writing file"); }
@ -1598,13 +1612,106 @@ namespace ArdupilotMega.GCSViews
private void BUT_loadwpfile_Click(object sender, EventArgs e)
{
OpenFileDialog fd = new OpenFileDialog();
fd.Filter = "Ardupilot Mission (*.h)|*.*";
fd.DefaultExt = ".h";
fd.Filter = "Ardupilot Mission (*.txt)|*.*";
fd.DefaultExt = ".txt";
DialogResult result = fd.ShowDialog();
string file = fd.FileName;
if (file != "")
{
readwaypointwritterfile(file);
if (file.ToLower().EndsWith(".h"))
{
readwaypointwritterfile(file);
}
else
{
readQGC110wpfile(file);
}
}
}
void readQGC110wpfile(string file)
{
int wp_count = 0;
bool error = false;
List<Locationwp> cmds = new List<Locationwp>();
try
{
StreamReader sr = new StreamReader(file); //"defines.h"
string header = sr.ReadLine();
if (header == null || !header.Contains("QGC WPL 110"))
{
MessageBox.Show("Invalid Waypoint file");
return;
}
while (!error && !sr.EndOfStream)
{
string line = sr.ReadLine();
// waypoints
if (line.StartsWith("#"))
continue;
string[] items = line.Split(new char[] {(char)'\t',' '}, StringSplitOptions.RemoveEmptyEntries);
if (items.Length <= 9)
continue;
try
{
Locationwp temp = new Locationwp();
if (items[2] == "3")
{ // abs MAV_FRAME_GLOBAL_RELATIVE_ALT=3
temp.options = 1;
} else
{
temp.options = 0;
}
temp.id = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), items[3], false);
temp.p1 = (byte)float.Parse(items[4]);
if (temp.id == 99)
temp.id = 0;
if (temp.id < (byte)MAVLink.MAV_CMD.LAST || wp_count == 0)
{
temp.alt = (int)(double.Parse(items[10], new System.Globalization.CultureInfo("en-US")) * 100);
temp.lat = (int)(double.Parse(items[8], new System.Globalization.CultureInfo("en-US")) * 10000000);
temp.lng = (int)(double.Parse(items[9], new System.Globalization.CultureInfo("en-US")) * 10000000);
}
else
{
temp.alt = (int)(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")));
temp.lat = (int)(double.Parse(items[6], new System.Globalization.CultureInfo("en-US")));
temp.lng = (int)(double.Parse(items[7], new System.Globalization.CultureInfo("en-US")));
}
cmds.Add(temp);
wp_count++;
}
catch { MessageBox.Show("Line invalid\n" + line); }
if (wp_count == byte.MaxValue)
{
MessageBox.Show("To many Waypoints!!!");
break;
}
}
sr.Close();
processToScreen(cmds);
writeKML();
MainMap.ZoomAndCenterMarkers("objects");
}
catch (Exception ex)
{
MessageBox.Show("Can't open file! " + ex.ToString());
}
}

View File

@ -225,12 +225,20 @@ namespace ArdupilotMega.HIL
#else
gps.alt = ((float)(altitude));
gps.fix_type = 3;
gps.v = ((float)Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)));
gps.hdg = (float)(((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360); ;
gps.lat = ((float)latitude);
gps.lon = ((float)longitude);
gps.usec = ((ulong)DateTime.Now.Ticks);
//Random rand = new Random();
//gps.alt += (rand.Next(100) - 50) / 100.0f;
//gps.lat += (float)((rand.NextDouble() - 0.5) * 0.00005);
//gps.lon += (float)((rand.NextDouble() - 0.5) * 0.00005);
//gps.v += (float)(rand.NextDouble() - 0.5) * 1.0f;
gps.v = ((float)Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)));
gps.hdg = (float)(((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360); ;
asp.airspeed = gps.v;
#endif
@ -244,7 +252,7 @@ namespace ArdupilotMega.HIL
MainV2.comPort.sendPacket(asp);
if (framecount % 10 == 0)
if (framecount % 12 == 0)
{// 50 / 10 = 5 hz
MainV2.comPort.sendPacket(gps);
//Console.WriteLine(DateTime.Now.Millisecond + " GPS" );

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.91")]
[assembly: AssemblyFileVersion("1.0.92")]
[assembly: NeutralResourcesLanguageAttribute("")]

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>dszV+o57dCwksPcpzEBhuhMS3T8=</dsig:DigestValue>
<dsig:DigestValue>tWULF9sdqZl3KU4VfKfZtd5uMkY=</dsig:DigestValue>
</hash>
</dependentAssembly>
</dependency>