/// <summary> /// this constructor is used only for reading ardupilot mission files /// </summary> /// <param name="lw"></param> internal Trackpoint(Locationwp lw) : base(lw.lat, lw.lng, lw.alt) { trackpointState = TrackpointState.None; number = lw.number; id = (MAV_CMD)Enum.Parse(typeof(MAV_CMD), lw.id.ToString()); // command id isHome = lw.ishome != 0; coordinateFrameOption = lw.options == 1 ? CoordinateFrameOption.MAV_FRAME_GLOBAL_RELATIVE_ALT : CoordinateFrameOption.MAV_FRAME_GLOBAL; // alt is in meters, can be above the ground (AGL) or above mean sea level (MSL), depending on coordinateFrameOption geoPosition = new GeoPosition(lw.lng, lw.lat, lw.alt); // p1-p4 are just float point numbers that can be added to waypoints and be interpreted by behaviors. We pass them all directly. p1 = lw.p1; p2 = lw.p2; p3 = lw.p3; p4 = lw.p4; }
/// <summary> /// this constructor is used only for reading mission files /// </summary> /// <param name="lw"></param> internal Trackpoint(Locationwp lw) : base(lw.lat, lw.lng, lw.alt) { trackpointState = TrackpointState.None; number = lw.number; id = (MAV_CMD)Enum.Parse(typeof(MAV_CMD), lw.id.ToString()); // command id isHome = lw.ishome != 0; coordinateFrameOption = lw.options == 1 ? CoordinateFrameOption.MAV_FRAME_GLOBAL_RELATIVE_ALT : CoordinateFrameOption.MAV_FRAME_GLOBAL; // alt is in meters, can be above the ground (AGL) or above mean sea level (MSL), depending on coordinateFrameOption geoPosition = new GeoPosition(lw.lng, lw.lat, lw.alt); // p1-p4 are just float point numbers that can be added to waypoints and be interpreted by behaviors. We pass them all directly. p1 = lw.p1; p2 = lw.p2; p3 = lw.p3; p4 = lw.p4; }
/// <summary> /// see http://ardupilot.org/planner/docs/common-install-mission-planner.html for track planning utility /// see C:\Program Files (x86)\APM Planner /// see C:\Projects\Win10\Missions /// (old) see C:\Projects\Robotics\DIY_Drones\ArduPlane-2.40\ArduPlane-2.40\Tools\ArdupilotMegaPlanner\GCSViews\FlightPlanner.cs line 1786, 1144 /// </summary> /// <param name="missionFileName"></param> private void readQGC110wpfile() { int wp_count = 0; bool error = false; string identLine = "QGC WPL 110"; Debug.WriteLine("IP: readQGC110wpfile() missionFileName=" + trackFileName); try { using (FileStream fs = new FileStream(trackFileName, FileMode.Open, FileAccess.Read)) { using (StreamReader sr = new StreamReader(fs)) { string header = sr.ReadLine(); if (header == null || !header.Contains(identLine)) { Debug.WriteLine("Invalid Waypoint file '" + trackFileName + "' - must contain first line '" + identLine + "'"); return; } System.Globalization.CultureInfo cultureInfo = new System.Globalization.CultureInfo("en-US"); while (!error && !sr.EndOfStream) { string line = sr.ReadLine(); // trackpoints if (line.StartsWith("#")) { continue; } string[] items = line.Split(new char[] { (char)'\t', ' ' }, StringSplitOptions.RemoveEmptyEntries); if (items.Length <= 9) { continue; } try { Locationwp temp = new Locationwp(); temp.number = int.Parse(items[0]); temp.ishome = byte.Parse(items[1]); if (items[2] == "3") { // normally altitute is above mean sea level (MSL), or relative to ground (AGL) when MAV_FRAME_GLOBAL_RELATIVE_ALT=3 temp.options = 1; } else { temp.options = 0; } temp.id = (byte)(int)Enum.Parse(typeof(MAV_CMD), items[3], false); temp.p1 = float.Parse(items[4], cultureInfo); if (temp.id == 99) { temp.id = 0; } temp.alt = (float)(double.Parse(items[10], cultureInfo)); temp.lat = (float)(double.Parse(items[8], cultureInfo)); temp.lng = (float)(double.Parse(items[9], cultureInfo)); temp.p2 = (float)(double.Parse(items[5], cultureInfo)); temp.p3 = (float)(double.Parse(items[6], cultureInfo)); temp.p4 = (float)(double.Parse(items[7], cultureInfo)); trackpoints.Add(new Trackpoint(temp)); wp_count++; } catch { Debug.WriteLine("Error: Line invalid: " + line); } if (wp_count == byte.MaxValue) { Debug.WriteLine("Error: Too many trackpoints!!! - limited to " + byte.MaxValue); break; } } } } Debug.WriteLine("OK: readQGC110wpfile() done, waypoint count=" + trackpoints.Count); } catch (Exception ex) { Debug.WriteLine("Error: Can't open file: '" + trackFileName + "'" + ex.ToString()); throw; } }
/// <summary> /// see http://ardupilot.org/planner/docs/common-install-mission-planner.html for track planning utility /// see C:\Program Files (x86)\APM Planner /// see C:\Projects\Win10\Missions /// (old) see C:\Projects\Robotics\DIY_Drones\ArduPlane-2.40\ArduPlane-2.40\Tools\ArdupilotMegaPlanner\GCSViews\FlightPlanner.cs line 1786, 1144 /// </summary> /// <param name="missionFileName"></param> private void readQGC110wpfile() { int wp_count = 0; bool error = false; string identLine = "QGC WPL 110"; Debug.WriteLine("IP: readQGC110wpfile() missionFileName=" + trackFileName); try { using (FileStream fs = new FileStream(trackFileName, FileMode.Open, FileAccess.Read)) { using (StreamReader sr = new StreamReader(fs)) { string header = sr.ReadLine(); if (header == null || !header.Contains(identLine)) { Debug.WriteLine("Invalid Waypoint file '" + trackFileName + "' - must contain first line '" + identLine + "'"); return; } System.Globalization.CultureInfo cultureInfo = new System.Globalization.CultureInfo("en-US"); while (!error && !sr.EndOfStream) { string line = sr.ReadLine(); // trackpoints if (line.StartsWith("#")) continue; string[] items = line.Split(new char[] { (char)'\t', ' ' }, StringSplitOptions.RemoveEmptyEntries); if (items.Length <= 9) continue; try { Locationwp temp = new Locationwp(); temp.number = int.Parse(items[0]); temp.ishome = byte.Parse(items[1]); if (items[2] == "3") { // normally altitute is above mean sea level (MSL), or relative to ground (AGL) when MAV_FRAME_GLOBAL_RELATIVE_ALT=3 temp.options = 1; } else { temp.options = 0; } temp.id = (byte)(int)Enum.Parse(typeof(MAV_CMD), items[3], false); temp.p1 = float.Parse(items[4], cultureInfo); if (temp.id == 99) temp.id = 0; temp.alt = (float)(double.Parse(items[10], cultureInfo)); temp.lat = (float)(double.Parse(items[8], cultureInfo)); temp.lng = (float)(double.Parse(items[9], cultureInfo)); temp.p2 = (float)(double.Parse(items[5], cultureInfo)); temp.p3 = (float)(double.Parse(items[6], cultureInfo)); temp.p4 = (float)(double.Parse(items[7], cultureInfo)); trackpoints.Add(new Trackpoint(temp)); wp_count++; } catch { Debug.WriteLine("Error: Line invalid: " + line); } if (wp_count == byte.MaxValue) { Debug.WriteLine("Error: Too many trackpoints!!! - limited to " + byte.MaxValue); break; } } } } Debug.WriteLine("OK: readQGC110wpfile() done, waypoint count=" + trackpoints.Count); } catch (Exception ex) { Debug.WriteLine("Error: Can't open file: '" + trackFileName + "'" + ex.ToString()); throw; } }