diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index cd1cab5c53..c0f99f4cab 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -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 { diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index ba81d91b9e..da53a6ddbc 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -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) diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index 32a355422d..4b4e329bb3 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -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 cmds = new List(); + + 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()); } } diff --git a/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs b/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs index 2b9854d3ad..5149771eda 100644 --- a/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs +++ b/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs @@ -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" ); diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index a04e8a5a7d..298fc58270 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.91")] +[assembly: AssemblyFileVersion("1.0.92")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application index b9ed0e421e..0ac9171517 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application +++ b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application @@ -11,7 +11,7 @@ - dszV+o57dCwksPcpzEBhuhMS3T8= + tWULF9sdqZl3KU4VfKfZtd5uMkY=