public static void SortLogs(string[] logs) { foreach (var logfile in logs) { FileInfo info = new FileInfo(logfile); if (info.Length <= 1024) { try { string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "SMALL" + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) Directory.CreateDirectory(destdir); File.Move(logfile, destdir + Path.GetFileName(logfile)); File.Move(logfile.Replace(".tlog", ".rlog"), destdir + Path.GetFileName(logfile).Replace(".tlog", ".rlog")); } catch { } continue; } MAVLinkInterface mine = new MAVLinkInterface(); try { using (mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))) { mine.logreadmode = true; byte[] hbpacket = mine.getHeartBeat(); if (hbpacket.Length == 0) continue; MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket); mine.logreadmode = false; mine.logplaybackfile.Close(); string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) Directory.CreateDirectory(destdir); File.Move(logfile, destdir + Path.GetFileName(logfile)); try { File.Move(logfile.Replace(".tlog", ".rlog"), destdir + Path.GetFileName(logfile).Replace(".tlog", ".rlog")); } catch { } } } catch { continue; } } }
private void BUT_connect_Click(object sender, EventArgs e) { Comms.CommsSerialScan.Scan(false); DateTime deadline = DateTime.Now.AddSeconds(50); while (Comms.CommsSerialScan.foundport == false) { System.Threading.Thread.Sleep(100); if (DateTime.Now > deadline) { CustomMessageBox.Show("Timeout waiting for autoscan/no mavlink device connected"); return; } } MAVLinkInterface com2 = new MAVLinkInterface(); com2.BaseStream.PortName = Comms.CommsSerialScan.portinterface.PortName; com2.BaseStream.BaudRate = Comms.CommsSerialScan.portinterface.BaudRate; com2.Open(true); MainV2.Comports.Add(com2); bindingSource1.ResetBindings(false); }
public OpticalFlow(MAVLinkInterface mav) { _mav = mav; subDataTrans = mav.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE, ReceviedPacket); subEncapData = mav.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA, ReceviedPacket); }
public MAVLinkSerialPort(MAVLinkInterface mavint, MAVLink.SERIAL_CONTROL_DEV port) { this.mavint = mavint; this.port = port; if (!mavint.BaseStream.IsOpen) { mavint.BaseStream.Open(); } if (mavint.getHeartBeat().Length == 0) { throw new Exception("No valid heartbeats read from port"); } if (subscription.Value != null) mavint.UnSubscribeToPacketType(subscription); subscription = mavint.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SERIAL_CONTROL, ReceviedPacket, true); bgdata = new Thread(mainloop); bgdata.Name = "MAVLinkSerialPort"; bgdata.IsBackground = true; bgdata.Start(); }
public TerrainFollow(MAVLinkInterface inInterface) { _interface = inInterface; log.Info("Subscribe to packets"); subscription = _interface.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST, ReceviedPacket); }
public TerrainFollow(MAVLinkInterface inInterface) { _interface = inInterface; log.Info("Subscribe to packets"); subscription = _interface.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.TERRAIN_DATA, ReceviedPacket); mFlightData = FlightData.instance; }
public static void doUpload(string file) { if (!validcred) { doUserAndPassword(); } string droneshareusername = MainV2.getConfig("droneshareusername"); string dronesharepassword = MainV2.getConfig("dronesharepassword"); if (dronesharepassword != "") { try { // fail on bad entry var crypto = new Crypto(); dronesharepassword = crypto.DecryptString(dronesharepassword); } catch { } } MAVLinkInterface mav = new MAVLinkInterface(); mav.BaseStream = new Comms.CommsFile(); mav.BaseStream.PortName = file; mav.BaseStream.Open(); if (mav.getHeartBeat().Length == 0) { CustomMessageBox.Show("Invalid log"); return; } mav.Close(); string viewurl = Utilities.DroneApi.droneshare.doUpload(file, droneshareusername, dronesharepassword, mav.MAV.Guid , Utilities.DroneApi.APIConstants.apiKey); if (viewurl != "") { try { validcred = true; System.Diagnostics.Process.Start(viewurl); } catch (Exception ex) { log.Error(ex); CustomMessageBox.Show("Failed to open url " + viewurl); } } }
void processbg(string file) { if (!File.Exists(file + ".jpg")) { LogMap.MapLogs(new string[] {file}); } var loginfo = new loginfo(); loginfo.fullname = file; loginfo.Size = new FileInfo(file).Length; if (File.Exists(file + ".jpg")) { loginfo.img = new Bitmap(file + ".jpg"); } if (file.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) { try { mine.logplaybackfile = new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; } mine.logreadmode = true; mine.getHeartBeat(); loginfo.Date = mine.lastlogread; loginfo.Aircraft = mine.sysidcurrent; var start = mine.lastlogread; try { if (mine.logplaybackfile.BaseStream.Length > MaxPlaybackLogSize) { mine.logplaybackfile.BaseStream.Seek(-100000, SeekOrigin.End); } else { mine.logplaybackfile.BaseStream.Seek(0, SeekOrigin.Begin); } } catch { } var end = mine.lastlogread; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { mine.readPacket(); if (mine.lastlogread > end) end = mine.lastlogread; } loginfo.Duration = (end - start).ToString(); } } logs.Add(loginfo); }
private static void RequestCallback(IAsyncResult ar) { TcpClient client = (TcpClient)ar.AsyncState; byte localsysid = newsysid++; if (client.Connected) { MAVLinkInterface mav = new MAVLinkInterface(); mav.BaseStream = new TcpSerial() { client = client }; try { mav.GetParam("SYSID_THISMAV"); } catch { } var ans = mav.setParam("SYSID_THISMAV", localsysid); Console.WriteLine("this mav set " + ans); mav = null; clients.Add(client); } }
public static void MapLogs(string[] logs) { foreach (var logfile in logs) { if (File.Exists(logfile + ".jpg")) continue; double minx = 99999; double maxx = -99999; double miny = 99999; double maxy = -99999; bool sitl = false; Dictionary<int,List<PointLatLngAlt>> loc_list = new Dictionary<int, List<PointLatLngAlt>>(); try { if (logfile.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) using ( mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) ) { mine.logreadmode = true; mine.speechenabled = false; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { MAVLink.MAVLinkMessage packet = mine.readPacket(); if (packet.Length < 5) continue; if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.SIM_STATE || packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.SIMSTATE) { sitl = true; } if (packet.msgid == (byte) MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT) { var loc = packet.ToStructure<MAVLink.mavlink_global_position_int_t>(); if (loc.lat == 0 || loc.lon == 0) continue; var id = MAVList.GetID(packet.sysid, packet.compid); if (!loc_list.ContainsKey(id)) loc_list[id] = new List<PointLatLngAlt>(); loc_list[id].Add(new PointLatLngAlt(loc.lat/10000000.0f, loc.lon/10000000.0f)); minx = Math.Min(minx, loc.lon/10000000.0f); maxx = Math.Max(maxx, loc.lon/10000000.0f); miny = Math.Min(miny, loc.lat/10000000.0f); maxy = Math.Max(maxy, loc.lat/10000000.0f); } } } } else if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log")) { bool bin = logfile.ToLower().EndsWith(".bin"); BinaryLog binlog = new BinaryLog(); DFLog dflog = new DFLog(); using (var st = File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) { using (StreamReader sr = new StreamReader(st)) { loc_list[0] = new List<PointLatLngAlt>(); while (sr.BaseStream.Position < sr.BaseStream.Length) { string line = ""; if (bin) { line = binlog.ReadMessage(sr.BaseStream); } else { line = sr.ReadLine(); } if (line.StartsWith("FMT")) { dflog.FMTLine(line); } else if (line.StartsWith("GPS")) { var item = dflog.GetDFItemFromLine(line, 0); if (!dflog.logformat.ContainsKey("GPS")) continue; var status = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Status")]); var lat = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lat")]); var lon = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lng")]); if (lat == 0 || lon == 0 || status < 3) continue; loc_list[0].Add(new PointLatLngAlt(lat, lon)); minx = Math.Min(minx, lon); maxx = Math.Max(maxx, lon); miny = Math.Min(miny, lat); maxy = Math.Max(maxy, lat); } } } } } if (loc_list.Count > 0 && loc_list.First().Value.Count > 10) { // add a bit of buffer var area = RectLatLng.FromLTRB(minx - 0.001, maxy + 0.001, maxx + 0.001, miny - 0.001); var map = GetMap(area); var grap = Graphics.FromImage(map); if (sitl) { AddTextToMap(grap, "SITL"); } Color[] colours = { Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo, Color.Violet, Color.Pink }; int a = 0; foreach (var locs in loc_list.Values) { PointF lastpoint = new PointF(); var pen = new Pen(colours[a%(colours.Length - 1)]); foreach (var loc in locs) { PointF newpoint = GetPixel(area, loc, map.Size); if (!lastpoint.IsEmpty) grap.DrawLine(pen, lastpoint, newpoint); lastpoint = newpoint; } a++; } map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg); map.Dispose(); map = null; } else { DoTextMap(logfile + ".jpg", "No gps data"); } } catch (Exception ex) { if (ex.ToString().Contains("Mavlink 0.9")) DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9"); continue; } } }
void dolog() { flightdata.Clear(); MAVLinkInterface mine = new MAVLinkInterface(); try { mine.logplaybackfile = new BinaryReader(File.Open(txt_tlog.Text, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch { CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; } mine.logreadmode = true; mine.MAV.packets.Initialize(); // clear mine.readPacket(); startlogtime = mine.lastlogread; double oldlatlngsum = 0; int appui = 0; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); cs.datetime = mine.lastlogread; cs.UpdateCurrentSettings(null, true, mine); if (appui != DateTime.Now.Second) { // cant do entire app as mixes with flightdata timer this.Refresh(); appui = DateTime.Now.Second; } try { if (MainV2.speechEngine != null) { MainV2.speechEngine.SpeakAsyncCancelAll(); } } catch { } // ignore because of this Exception System.PlatformNotSupportedException: No voice installed on the system or none available with the current security setting. // if ((float)(cs.lat + cs.lng + cs.alt) != oldlatlngsum // && cs.lat != 0 && cs.lng != 0) DateTime nexttime = mine.lastlogread.AddMilliseconds(-(mine.lastlogread.Millisecond % 100)); if (!flightdata.ContainsKey(nexttime)) { Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " + (float)(cs.lat + cs.lng + cs.alt) + "!=" + oldlatlngsum); CurrentState cs2 = (CurrentState)cs.Clone(); try { flightdata.Add(nexttime, cs2); } catch { } oldlatlngsum = (cs.lat + cs.lng + cs.alt); } } mine.logreadmode = false; mine.logplaybackfile.Close(); mine.logplaybackfile = null; }
public void DownloadFence(MAVLinkInterface port, Action <int, string> progress = null) { var list = mav_mission.download(port, MAVLink.MAV_MISSION_TYPE.FENCE, progress); LocationToFence(list); }
public static void MapLogs(string[] logs) { foreach (var logfile in logs) { if (File.Exists(logfile + ".jpg")) continue; double minx = 99999; double maxx = -99999; double miny = 99999; double maxy = -99999; List<PointLatLngAlt> locs = new List<PointLatLngAlt>(); try { if (logfile.ToLower().EndsWith(".tlog")) { MAVLinkInterface mine = new MAVLinkInterface(); using (mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))) { mine.logreadmode = true; CurrentState cs = new CurrentState(); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); if (packet.Length < 5) continue; try { if (MainV2.speechEngine != null) MainV2.speechEngine.SpeakAsyncCancelAll(); } catch { } if (packet[5] == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT) { var loc = packet.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6); if (loc.lat == 0 || loc.lon == 0) continue; locs.Add(new PointLatLngAlt(loc.lat / 10000000.0f, loc.lon / 10000000.0f)); minx = Math.Min(minx, loc.lon / 10000000.0f); maxx = Math.Max(maxx, loc.lon / 10000000.0f); miny = Math.Min(miny, loc.lat / 10000000.0f); maxy = Math.Max(maxy, loc.lat / 10000000.0f); } } } } else if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log")) { bool bin = logfile.ToLower().EndsWith(".bin"); using (var st = File.OpenRead(logfile)) { using (StreamReader sr = new StreamReader(st)) { while (sr.BaseStream.Position < sr.BaseStream.Length) { string line = ""; if (bin) { line = BinaryLog.ReadMessage(sr.BaseStream); } else { line = sr.ReadLine(); } if (line.StartsWith("FMT")) { DFLog.FMTLine(line); } else if (line.StartsWith("GPS")) { var item = DFLog.GetDFItemFromLine(line, 0); var lat = double.Parse(item.items[DFLog.FindMessageOffset(item.msgtype, "Lat")]); var lon = double.Parse(item.items[DFLog.FindMessageOffset(item.msgtype, "Lng")]); if (lat == 0 || lon == 0) continue; locs.Add(new PointLatLngAlt(lat, lon)); minx = Math.Min(minx, lon); maxx = Math.Max(maxx, lon); miny = Math.Min(miny, lat); maxy = Math.Max(maxy, lat); } } } } } if (locs.Count > 10) { // add a bit of buffer var area = RectLatLng.FromLTRB(minx - 0.001, maxy + 0.001, maxx + 0.001, miny - 0.001); var map = GetMap(area); var grap = Graphics.FromImage(map); PointF lastpoint = new PointF(); foreach (var loc in locs) { PointF newpoint = GetPixel(area, loc, map.Size); if (!lastpoint.IsEmpty) grap.DrawLine(Pens.Red, lastpoint, newpoint); lastpoint = newpoint; } map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg); map.Dispose(); map = null; } else { var map = new Bitmap(100, 100); var grap = Graphics.FromImage(map); grap.DrawString("No gps data", SystemFonts.DefaultFont, Brushes.Red, 0, 0, StringFormat.GenericDefault); map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg); map.Dispose(); map = null; } } catch { continue; } } }
public static void SortLogs(string[] logs, string masterdestdir = "") { Parallel.ForEach(logs, logfile => //foreach (var logfile in logs) { if (masterdestdir == "") { masterdestdir = Path.GetDirectoryName(logfile); } issitl = false; FileInfo info = new FileInfo(logfile); // delete 0 size files if (info.Length == 0) { try { File.Delete(logfile); } catch { } return; } // move small logs - most likerly invalid if (info.Length <= 1024) { try { string destdir = masterdestdir + Path.DirectorySeparatorChar + "SMALL" + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) { Directory.CreateDirectory(destdir); } log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile)); MoveFileUsingMask(logfile, destdir); } catch { } return; } try { using (MAVLinkInterface mine = new MAVLinkInterface()) using ( mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))) { mine.logreadmode = true; mine.speechenabled = false; var midpoint = mine.logplaybackfile.BaseStream.Length / 2; mine.logplaybackfile.BaseStream.Seek(midpoint, SeekOrigin.Begin); // used for sitl detection mine.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SIMSTATE, SitlDetection); mine.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SIM_STATE, SitlDetection); MAVLink.MAVLinkMessage hbpacket = mine.getHeartBeat(); MAVLink.MAVLinkMessage hbpacket1 = mine.getHeartBeat(); MAVLink.MAVLinkMessage hbpacket2 = mine.getHeartBeat(); MAVLink.MAVLinkMessage hbpacket3 = mine.getHeartBeat(); if (hbpacket.Length == 0 && hbpacket1.Length == 0 && hbpacket2.Length == 0 && hbpacket3.Length == 0) { mine.logreadmode = false; mine.logplaybackfile.Close(); if (!Directory.Exists(masterdestdir + Path.DirectorySeparatorChar + "BAD")) { Directory.CreateDirectory(masterdestdir + Path.DirectorySeparatorChar + "BAD"); } log.Info("Move log bad " + logfile + " to " + masterdestdir + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar + Path.GetFileName(logfile)); MoveFileUsingMask(logfile, masterdestdir + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar); return; } if (hbpacket.Length != 0) { MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket); } if (hbpacket1.Length != 0) { MAVLink.mavlink_heartbeat_t hb1 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket1); } if (hbpacket2.Length != 0) { MAVLink.mavlink_heartbeat_t hb2 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket2); } if (hbpacket3.Length != 0) { MAVLink.mavlink_heartbeat_t hb3 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket3); } // find most appropriate if (mine.MAVlist.Count > 1) { foreach (var mav in mine.MAVlist) { if (mav.aptype == MAVLink.MAV_TYPE.ANTENNA_TRACKER) { continue; } if (mav.aptype == MAVLink.MAV_TYPE.GCS) { continue; } mine.sysidcurrent = mav.sysid; mine.compidcurrent = mav.compid; } } mine.logreadmode = false; mine.logplaybackfile.Close(); string destdir = masterdestdir + Path.DirectorySeparatorChar + mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar + mine.MAV.sysid + Path.DirectorySeparatorChar; if (issitl) { destdir = masterdestdir + Path.DirectorySeparatorChar + "SITL" + Path.DirectorySeparatorChar + mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar + mine.MAV.sysid + Path.DirectorySeparatorChar; } if (!Directory.Exists(destdir)) { Directory.CreateDirectory(destdir); } MoveFileUsingMask(logfile, destdir); } } catch { return; } }); }
public static void MapLogs(string[] logs) { foreach (var logfile in logs) { if (File.Exists(logfile + ".jpg")) { continue; } double minx = 99999; double maxx = -99999; double miny = 99999; double maxy = -99999; List <PointLatLngAlt> locs = new List <PointLatLngAlt>(); try { if (logfile.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) using ( mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) ) { mine.logreadmode = true; CurrentState cs = new CurrentState(); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { MAVLink.MAVLinkMessage packet = mine.readPacket(); if (packet.Length < 5) { continue; } try { if (MainV2.speechEngine != null) { MainV2.speechEngine.SpeakAsyncCancelAll(); } } catch { } if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT) { var loc = packet.ByteArrayToStructure <MAVLink.mavlink_global_position_int_t>(); if (loc.lat == 0 || loc.lon == 0) { continue; } locs.Add(new PointLatLngAlt(loc.lat / 10000000.0f, loc.lon / 10000000.0f)); minx = Math.Min(minx, loc.lon / 10000000.0f); maxx = Math.Max(maxx, loc.lon / 10000000.0f); miny = Math.Min(miny, loc.lat / 10000000.0f); maxy = Math.Max(maxy, loc.lat / 10000000.0f); } } } } else if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log")) { bool bin = logfile.ToLower().EndsWith(".bin"); BinaryLog binlog = new BinaryLog(); DFLog dflog = new DFLog(); using (var st = File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) { using (StreamReader sr = new StreamReader(st)) { while (sr.BaseStream.Position < sr.BaseStream.Length) { string line = ""; if (bin) { line = binlog.ReadMessage(sr.BaseStream); } else { line = sr.ReadLine(); } if (line.StartsWith("FMT")) { dflog.FMTLine(line); } else if (line.StartsWith("GPS")) { var item = dflog.GetDFItemFromLine(line, 0); var lat = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lat")]); var lon = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lng")]); if (lat == 0 || lon == 0) { continue; } locs.Add(new PointLatLngAlt(lat, lon)); minx = Math.Min(minx, lon); maxx = Math.Max(maxx, lon); miny = Math.Min(miny, lat); maxy = Math.Max(maxy, lat); } } } } } if (locs.Count > 10) { // add a bit of buffer var area = RectLatLng.FromLTRB(minx - 0.001, maxy + 0.001, maxx + 0.001, miny - 0.001); var map = GetMap(area); var grap = Graphics.FromImage(map); PointF lastpoint = new PointF(); foreach (var loc in locs) { PointF newpoint = GetPixel(area, loc, map.Size); if (!lastpoint.IsEmpty) { grap.DrawLine(Pens.Red, lastpoint, newpoint); } lastpoint = newpoint; } map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg); map.Dispose(); map = null; } else { DoTextMap(logfile + ".jpg", "No gps data"); } } catch (Exception ex) { if (ex.ToString().Contains("Mavlink 0.9")) { DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9"); } continue; } } }
public static void doUpload(string file) { doUserAndPassword(); string droneshareusername = MainV2.getConfig("droneshareusername"); string dronesharepassword = MainV2.getConfig("dronesharepassword"); if (dronesharepassword != "") { try { // fail on bad entry var crypto = new Crypto(); dronesharepassword = crypto.DecryptString(dronesharepassword); } catch { } } MAVLinkInterface mav = new MAVLinkInterface(); mav.BaseStream = new Comms.CommsFile(); mav.BaseStream.PortName = file; mav.getHeartBeat(); mav.Close(); string guid = Guid.NewGuid().ToString(); switch (mav.MAV.cs.firmware) { case MainV2.Firmwares.ArduCopter2: guid = MainV2.config["copter_guid"].ToString(); break; case MainV2.Firmwares.ArduPlane: guid = MainV2.config["plane_guid"].ToString(); break; case MainV2.Firmwares.ArduRover: guid = MainV2.config["rover_guid"].ToString(); break; } string viewurl = Utilities.droneshare.doUpload(file, droneshareusername, dronesharepassword, guid, Utilities.droneshare.APIConstants.apiKey); if (viewurl != "") { try { System.Diagnostics.Process.Start(viewurl); } catch (Exception ex) { CustomMessageBox.Show("Failed to open url "+ viewurl); } } }
/// <summary> /// Processes a tlog to get the offsets - creates dxf of data /// </summary> /// <param name="fn">Filename</param> /// <returns>Offsets</returns> public static double[] getOffsets(string fn, int throttleThreshold = 0) { // based off tridge's work string logfile = fn; // old method float minx = 0; float maxx = 0; float miny = 0; float maxy = 0; float minz = 0; float maxz = 0; // this is for a dxf Polyline3dVertex vertex; List <Polyline3dVertex> vertexes = new List <Polyline3dVertex>(); // data storage Tuple <float, float, float> offset = new Tuple <float, float, float>(0, 0, 0); List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >(); Hashtable filter = new Hashtable(); // track data to use bool useData = false; if (throttleThreshold <= 0) { useData = true; } log.Info("Start log: " + DateTime.Now); MAVLinkInterface mine = new MAVLinkInterface(); try { mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return(new double[] { 0 }); } mine.logreadmode = true; mine.MAV.packets.Initialize(); // clear // gather data while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packetraw = mine.readPacket(); var packet = mine.DebugPacket(packetraw, false); // this is for packets we dont know about if (packet == null) { continue; } if (packet.GetType() == typeof(MAVLink.mavlink_vfr_hud_t)) { if (((MAVLink.mavlink_vfr_hud_t)packet).throttle >= throttleThreshold) { useData = true; } else { useData = false; } } if (packet.GetType() == typeof(MAVLink.mavlink_sensor_offsets_t)) { offset = new Tuple <float, float, float>( ((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_x, ((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_y, ((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_z); } else if (packet.GetType() == typeof(MAVLink.mavlink_raw_imu_t) && useData) { int div = 20; // fox dxf vertex = new Polyline3dVertex(new Vector3f( ((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1, ((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2, ((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3) ); vertexes.Add(vertex); // for old method setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1, ref minx, ref maxx); setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2, ref miny, ref maxy); setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3, ref minz, ref maxz); // for new lease sq string item = (int)(((MAVLink.mavlink_raw_imu_t)packet).xmag / div) + "," + (int)(((MAVLink.mavlink_raw_imu_t)packet).ymag / div) + "," + (int)(((MAVLink.mavlink_raw_imu_t)packet).zmag / div); if (filter.ContainsKey(item)) { filter[item] = (int)filter[item] + 1; if ((int)filter[item] > 3) { continue; } } else { filter[item] = 1; } data.Add(new Tuple <float, float, float>( ((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1, ((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2, ((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3)); } } log.Info("Log Processed " + DateTime.Now); Console.WriteLine("Extracted " + data.Count + " data points"); Console.WriteLine("Current offset: " + offset); mine.logreadmode = false; mine.logplaybackfile.Close(); mine.logplaybackfile = null; if (data.Count < 10) { CustomMessageBox.Show("Log does not contain enough data"); throw new Exception("Not Enough Data"); } data.Sort( delegate(Tuple <float, float, float> d1, Tuple <float, float, float> d2) { // get distance from 0,0,0 double ans1 = Math.Sqrt(d1.Item1 * d1.Item1 + d1.Item2 * d1.Item2 + d1.Item3 * d1.Item3); double ans2 = Math.Sqrt(d2.Item1 * d2.Item1 + d2.Item2 * d2.Item2 + d2.Item3 * d2.Item3); if (ans1 > ans2) { return(1); } if (ans1 < ans2) { return(-1); } return(0); } ); data.RemoveRange(data.Count - (data.Count / 16), data.Count / 16); System.Console.WriteLine("Old Method {0} {1} {2}", -(maxx + minx) / 2, -(maxy + miny) / 2, -(maxz + minz) / 2); double[] x = LeastSq(data); log.Info("Least Sq Done " + DateTime.Now); doDXF(vertexes, x); Array.Resize <double>(ref x, 3); return(x); }
private void start_Terminal(bool px4) { setcomport(); try { if (MainV2.comPort != null && MainV2.comPort.BaseStream != null && MainV2.comPort.BaseStream.IsOpen) { MainV2.comPort.BaseStream.Close(); } if (comPort.IsOpen) { Console.WriteLine("Terminal Start - Close Port"); threadrun = false; // if (DialogResult.Cancel == CustomMessageBox.Show("The port is open\n Continue?", "Continue", MessageBoxButtons.YesNo)) { // return; } comPort.Close(); // allow things to cleanup Thread.Sleep(400); } comPort.ReadBufferSize = 1024 * 1024 * 4; comPort.PortName = MainV2.comPortName; // test moving baud rate line comPort.BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text); if (px4) { TXT_terminal.AppendText("Rebooting " + MainV2.comPortName + " at " + comPort.BaudRate + "\n"); // keep it local using (var mine = new MAVLinkInterface()) { mine.BaseStream.PortName = MainV2.comPortName; mine.BaseStream.BaudRate = comPort.BaudRate; mine.giveComport = true; mine.BaseStream.Open(); // check if we are a mavlink stream var buffer = mine.readPacket(); if (buffer.Length > 0) { log.Info("got packet - sending reboot via mavlink"); TXT_terminal.AppendText("Via Mavlink\n"); mine.doReboot(false, false); try { mine.BaseStream.Close(); } catch { } } else { log.Info("no packet - sending reboot via console"); TXT_terminal.AppendText("Via Console\n"); try { mine.BaseStream.Write("reboot\r"); mine.BaseStream.Write("exit\rreboot\r"); } catch { } try { mine.BaseStream.Close(); } catch { } } } TXT_terminal.AppendText("Waiting for reboot\n"); // wait 7 seconds for px4 reboot log.Info("waiting for reboot"); var deadline = DateTime.Now.AddSeconds(9); while (DateTime.Now < deadline) { Thread.Sleep(500); Application.DoEvents(); } var a = 0; while (a < 5) { try { if (!comPort.IsOpen) { comPort.Open(); } } catch { } Thread.Sleep(200); a++; } } else { log.Info("About to open " + comPort.PortName); comPort.Open(); log.Info("toggle dtr"); comPort.toggleDTR(); } try { comPort.DiscardInBuffer(); } catch { } startreadthread(); } catch (Exception ex) { log.Error(ex); TXT_terminal.AppendText("Cant open serial port\r\n"); return; } }
/// <summary> /// main serial reader thread /// controls /// serial reading /// link quality stats /// speech voltage - custom - alt warning - data lost /// heartbeat packet sending /// /// and can't fall out /// </summary> private void SerialReader() { if (serialThread == true) { return; } serialThread = true; SerialThreadrunner.Reset(); int minbytes = 0; int altwarningmax = 0; bool armedstatus = false; string lastmessagehigh = ""; DateTime speechcustomtime = DateTime.Now; DateTime speechbatterytime = DateTime.Now; DateTime speechlowspeedtime = DateTime.Now; DateTime linkqualitytime = DateTime.Now; while (serialThread) { try { Thread.Sleep(1); // was 5 //// get home point on armed status change. //if (armedstatus != Form1.comPort.MAV.cs.armed && comPort.BaseStream.IsOpen) //{ // armedstatus = Form1.comPort.MAV.cs.armed; // // status just changed to armed // if (Form1.comPort.MAV.cs.armed == true) // { // try // { // //Form1.comPort.MAV.cs.HomeLocation = new PointLatLngAlt(Form1.comPort.getWP(0)); // //if (MyView.current != null && MyView.current.Name == "FlightPlanner") // //{ // // // update home if we are on flight data tab // // FlightPlanner.updateHome(); // //} // } // catch // { // // dont hang this loop // this.BeginInvoke((MethodInvoker)delegate { MessageBox.Show("Failed to update home location"); }); // } // } //} // send a hb every seconds from gcs to ap if (heatbeatSend.Second != DateTime.Now.Second) { MAVLink.mavlink_heartbeat_t htb = new MAVLink.mavlink_heartbeat_t() { type = (byte)MAVLink.MAV_TYPE.GCS, autopilot = (byte)MAVLink.MAV_AUTOPILOT.INVALID, mavlink_version = 3, }; foreach (var port in Form1.Comports) { try { port.sendPacket(htb); } catch { } } heatbeatSend = DateTime.Now; } // if not connected or busy, sleep and loop if (!comPort.BaseStream.IsOpen || comPort.giveComport == true) { if (!comPort.BaseStream.IsOpen) { // check if other ports are still open foreach (var port in Comports) { if (port.BaseStream.IsOpen) { Console.WriteLine("Main comport shut, swapping to other mav"); comPort = port; break; } } } System.Threading.Thread.Sleep(100); //continue; } // actualy read the packets while (comPort.BaseStream.IsOpen && comPort.BaseStream.BytesToRead > minbytes && comPort.giveComport == false) { try { comPort.readPacket(); } catch { } } // update currentstate of sysids on main port foreach (var sysid in comPort.sysidseen) { try { comPort.MAVlist[sysid].cs.UpdateCurrentSettings(null, false, comPort, comPort.MAVlist[sysid]); } catch { } } //C_Log.writeLog(comPort.MAV.cs.pitch + " " + comPort.MAV.cs.roll + " " + comPort.MAV.cs.yaw + "\n\r"); this.Invoke((MethodInvoker) delegate { //update teks //lbl_test.Visible = true; //lbl_test.Text = "YAW :" + comPort.MAV.cs.yaw.ToString() + "\nAltitude :" + comPort.MAV.cs.alt.ToString(); //lbl_test.Text = "YAW :" + loop1.ToString() + "\nAltitude :" + loop2.ToString(); label1.Text = comPort.MAV.cs.yaw.ToString(); label2.Text = comPort.MAV.cs.pitch.ToString(); label3.Text = comPort.MAV.cs.roll.ToString(); }); ////Ngirim Data //if (USock != null) //{ // if (USock.isConnected()) // { // this.Invoke(sdtc); // } //} //// read the other interfaces //foreach (var port in Comports) //{ // // skip primary interface // if (port == comPort) // continue; // if (!port.BaseStream.IsOpen) // { // // modify array and drop out // Comports.Remove(port); // break; // } // while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes) // { // try // { // port.readPacket(); // } // catch { } // } // // update currentstate of sysids on the port // foreach (var sysid in port.sysidseen) // { // try // { // port.MAVlist[sysid].cs.UpdateCurrentSettings(null, false, port, port.MAVlist[sysid]); // } // catch { } // } //} //UPDATE JOYSTICK OUTPUT FROM SERIAL //updateProgressJoystickInputSERIAL(); //COMPARE DELAY //JoystickDelayCalculation(); } catch (Exception e) { log.Error("Serial Reader fail :" + e.ToString()); MessageBox.Show(e.ToString()); try { comPort.Close(); } catch { } } } Console.WriteLine("SerialReader Done"); SerialThreadrunner.Set(); }
public static void MapLogs(string[] logs) { foreach (var logfile in logs) { if (File.Exists(logfile + ".jpg") || logfile.Contains("Telemetry Logs")) continue; MAVLinkInterface mine = new MAVLinkInterface(); List<MAVLink.mavlink_global_position_int_t> locs = new List<MAVLink.mavlink_global_position_int_t>(); try { double minx = 99999; double maxx = -99999; double miny = 99999; double maxy = -99999; using (mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))) { mine.logreadmode = true; CurrentState cs = new CurrentState(); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); //Console.Write((mine.logplaybackfile.BaseStream.Position / (double)mine.logplaybackfile.BaseStream.Length) +"\r"); if (packet.Length < 5) continue; try { if (MainV2.speechEngine != null) MainV2.speechEngine.SpeakAsyncCancelAll(); } catch { } if (packet[5] == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT) { var loc = packet.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6); if (loc.lat == 0 || loc.lon == 0) continue; locs.Add(loc); minx = Math.Min(minx, loc.lon / 10000000.0f); maxx = Math.Max(maxx, loc.lon / 10000000.0f); miny = Math.Min(miny, loc.lat / 10000000.0f); maxy = Math.Max(maxy, loc.lat / 10000000.0f); } } } if (locs.Count > 10) { // add a bit of buffer var area = RectLatLng.FromLTRB(minx - 0.001 , maxy + 0.001, maxx + 0.001 , miny - 0.001 ); var map = GetMap(area); var grap = Graphics.FromImage(map); PointF lastpoint = new PointF(); foreach (var loc in locs) { PointF newpoint = GetPixel(area, loc, map.Size); if (!lastpoint.IsEmpty) grap.DrawLine(Pens.Red, lastpoint, newpoint); lastpoint = newpoint; } map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg); map.Dispose(); map = null; } else { var map = new Bitmap(100, 100); var grap = Graphics.FromImage(map); grap.DrawString("NO MAP", SystemFonts.DefaultFont, Brushes.Red, 0, 0, StringFormat.GenericDefault); map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg); map.Dispose(); map = null; } } catch { continue; } } }
public static PointLatLngAlt ProjectPoint(MAVLinkInterface comPort) { GimbalPoint.comPort = comPort; comPort.GetMountStatus(); // this should be looking at rc_channel function yawchannel = (int)(float)comPort.MAV.param["MNT_RC_IN_PAN"]; pitchchannel = (int)(float)comPort.MAV.param["MNT_RC_IN_TILT"]; rollchannel = (int)(float)comPort.MAV.param["MNT_RC_IN_ROLL"]; //if (!comPort.BaseStream.IsOpen) // return PointLatLngAlt.Zero; PointLatLngAlt currentlocation = new PointLatLngAlt(comPort.MAV.cs.lat, comPort.MAV.cs.lng); double yawangle = comPort.MAV.cs.campointc; double rollangle = comPort.MAV.cs.campointb; double pitchangle = comPort.MAV.cs.campointa; // if ((double)comPort.MAV.param["MNT_TYPE"] == 4) { yawangle = comPort.MAVlist[comPort.sysidcurrent, 67].cs.yaw; rollangle = comPort.MAVlist[comPort.sysidcurrent, 67].cs.roll; pitchangle = comPort.MAVlist[comPort.sysidcurrent, 67].cs.pitch; } if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0) { yawangle = ConvertPwmtoAngle(axis.yaw); //rollangle = ConvertPwmtoAngle(axis.roll); pitchangle = ConvertPwmtoAngle(axis.pitch) + comPort.MAV.cs.pitch; pitchangle -= Math.Sin(yawangle * MathHelper.deg2rad) * comPort.MAV.cs.roll; } // tan (o/a) // todo account for ground elevation change. int distout = 10; PointLatLngAlt newpos = PointLatLngAlt.Zero; //dist = Math.Tan((90 + pitchangle) * MathHelper.deg2rad) * (comPort.MAV.cs.alt); while (distout < 1000) { // get a projected point to test intersection against - not using slope distance PointLatLngAlt newposdist = currentlocation.newpos(yawangle + comPort.MAV.cs.yaw, distout); newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt; // get another point 50 infront PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + comPort.MAV.cs.yaw, distout + 50); newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt; // get the flat terrain distance out - at 0 alt double distflat = Math.Tan((90 + pitchangle) * MathHelper.deg2rad) * (comPort.MAV.cs.altasl); // x is dist from plane, y is alt var newpoint = FindLineIntersection(start1: new Point(0, comPort.MAV.cs.altasl), new Point((float)distflat, 0), new Point((float)distout, (float)newposdist.Alt), new Point((float)distout + 50, (float)newposdist2.Alt)); if (newpoint.X != 0) { newpos = newposdist2; break; } distout += 50; } //Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000")); //PointLatLngAlt newpos = currentlocation.newpos( yawangle + comPort.MAV.cs.yaw, dist); //Console.WriteLine(newpos); return(newpos); }
static string process(MAVLinkInterface mavint) { DateTime Deadline = DateTime.Now.AddSeconds(60); Vector3 last_mag = null; double last_usec = 0; double count = 0; double[] total_error = new double[rotations.Length]; float AHRS_ORIENTATION = 0; float COMPASS_ORIENT = 0; float COMPASS_EXTERNAL = 0; //# now gather all the data while (DateTime.Now < Deadline || mavint.BaseStream.BytesToRead > 0) { byte[] packetbytes = mavint.readPacket(); if (packetbytes.Length < 5) { continue; } object packet = mavint.GetPacket(packetbytes); if (packet == null) { continue; } if (packet is MAVLink.mavlink_param_value_t) { MAVLink.mavlink_param_value_t m = (MAVLink.mavlink_param_value_t)packet; if (str(m.param_id) == "AHRS_ORIENTATION") { AHRS_ORIENTATION = (int)(m.param_value); } if (str(m.param_id) == "COMPASS_ORIENT") { COMPASS_ORIENT = (int)(m.param_value); } if (str(m.param_id) == "COMPASS_EXTERNAL") { COMPASS_EXTERNAL = (int)(m.param_value); } } if (packet is MAVLink.mavlink_raw_imu_t) { MAVLink.mavlink_raw_imu_t m = (MAVLink.mavlink_raw_imu_t)packet; Vector3 mag = new Vector3(m.xmag, m.ymag, m.zmag); mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL); Vector3 gyr = new Vector3(m.xgyro, m.ygyro, m.zgyro) * 0.001; double usec = m.time_usec; if (last_mag != null && gyr.length() > radians(5.0)) { add_errors(mag, gyr, last_mag, (usec - last_usec) * 1.0e-6, total_error, rotations); count += 1; } last_mag = mag; last_usec = usec; } } int best_i = 0; double best_err = total_error[0]; foreach (var i in range(len(rotations))) { Rotation r = rotations[i]; // if (!r.is_90_degrees()) // continue; //if ( opts.verbose) { // print("%s err=%.2f" % (r, total_error[i]/count));} if (total_error[i] < best_err) { best_i = i; best_err = total_error[i]; } } Rotation rans = rotations[best_i]; Console.WriteLine("Best rotation is {0} err={1} from {2} points", rans, best_err / count, count); //print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count)); return(rans.name); }
private void start_Terminal(bool px4) { setcomport(); try { if (MainV2.comPort != null && MainV2.comPort.BaseStream != null && MainV2.comPort.BaseStream.IsOpen) MainV2.comPort.BaseStream.Close(); if (comPort.IsOpen) { Console.WriteLine("Terminal Start - Close Port"); threadrun = false; // if (DialogResult.Cancel == CustomMessageBox.Show("The port is open\n Continue?", "Continue", MessageBoxButtons.YesNo)) { // return; } comPort.Close(); // allow things to cleanup Thread.Sleep(400); } comPort.ReadBufferSize = 1024*1024*4; comPort.PortName = MainV2.comPortName; // test moving baud rate line comPort.BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text); if (px4) { TXT_terminal.AppendText("Rebooting " + MainV2.comPortName + " at " + comPort.BaudRate + "\n"); // keep it local using (var mine = new MAVLinkInterface()) { mine.BaseStream.PortName = MainV2.comPortName; mine.BaseStream.BaudRate = comPort.BaudRate; mine.giveComport = true; mine.BaseStream.Open(); // check if we are a mavlink stream var buffer = mine.readPacket(); if (buffer.Length > 0) { log.Info("got packet - sending reboot via mavlink"); TXT_terminal.AppendText("Via Mavlink\n"); mine.doReboot(false); try { mine.BaseStream.Close(); } catch { } } else { log.Info("no packet - sending reboot via console"); TXT_terminal.AppendText("Via Console\n"); try { mine.BaseStream.Write("reboot\r"); mine.BaseStream.Write("exit\rreboot\r"); } catch { } try { mine.BaseStream.Close(); } catch { } } } TXT_terminal.AppendText("Waiting for reboot\n"); // wait 7 seconds for px4 reboot log.Info("waiting for reboot"); var deadline = DateTime.Now.AddSeconds(9); while (DateTime.Now < deadline) { Thread.Sleep(500); Application.DoEvents(); } var a = 0; while (a < 5) { try { if (!comPort.IsOpen) comPort.Open(); } catch { } Thread.Sleep(200); a++; } } else { log.Info("About to open " + comPort.PortName); comPort.Open(); log.Info("toggle dtr"); comPort.toggleDTR(); } try { comPort.DiscardInBuffer(); } catch { } startreadthread(); } catch (Exception ex) { log.Error(ex); TXT_terminal.AppendText("Cant open serial port\r\n"); return; } }
public async Task StartSwarmSeperate() { var max = 10; if (InputBox.Show("how many?", "how many?", ref max) != DialogResult.OK) { return; } // kill old session try { simulator.ForEach(a => { try { a.Kill(); } catch { } }); } catch { } var exepath = CheckandGetSITLImage("ArduCopter.elf"); var model = "+"; var config = await GetDefaultConfig(model); max--; for (int a = (int)max; a >= 0; a--) { var extra = " --disable-fgview -r50 "; if (!string.IsNullOrEmpty(config)) { extra += @" --defaults """ + config + @",identity.parm"" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 "; } var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4); if (max == a) { extra += String.Format( " -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ", a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model); } else { extra += String.Format( " -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ", a, "" /*"--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a)*/, a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model); } string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar; Directory.CreateDirectory(simdir); File.WriteAllText(simdir + "identity.parm", String.Format(@"SERIAL0_PROTOCOL=2 SERIAL1_PROTOCOL=2 SYSID_THISMAV={0} SIM_TERRAIN=0 TERRAIN_ENABLE=0 SCHED_LOOP_RATE=50 SIM_RATE_HZ=400 SIM_DRIFT_SPEED=0 SIM_DRIFT_TIME=0 ", a + 1)); string path = Environment.GetEnvironmentVariable("PATH"); Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path, EnvironmentVariableTarget.Process); Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process); ProcessStartInfo exestart = new ProcessStartInfo(); exestart.FileName = await exepath; exestart.Arguments = extra; exestart.WorkingDirectory = simdir; exestart.WindowStyle = ProcessWindowStyle.Minimized; exestart.UseShellExecute = true; simulator.Add(System.Diagnostics.Process.Start(exestart)); } System.Threading.Thread.Sleep(2000); MainV2.View.ShowScreen(MainV2.View.screens[0].Name); try { for (int a = (int)max; a >= 0; a--) { var mav = new MAVLinkInterface(); var client = new Comms.TcpSerial(); client.client = new TcpClient("127.0.0.1", 5760 + (10 * (a))); mav.BaseStream = client; //SITLSEND = new UdpClient("127.0.0.1", 5501); Thread.Sleep(200); MainV2.instance.doConnect(mav, "preset", "5760", false); MainV2.Comports.Add(mav); } return; } catch { CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR); return; } }
public async void StartSwarmSeperate() { var exepath = CheckandGetSITLImage("ArduCopter.elf"); var model = "+"; var config = await GetDefaultConfig(model); var max = 10.0; if (InputBox.Show("how many?", "how many?", ref max) != DialogResult.OK) { return; } max--; for (int a = (int)max; a >= 0; a--) { var extra = " --disable-fgview -r50 "; if (!string.IsNullOrEmpty(config)) { extra += @" --defaults """ + config + @""" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 "; } var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4); if (max == a) { extra += String.Format( " -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ", a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model); } else { extra += String.Format( " -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ", a, "" /*"--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a)*/, a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model); } string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar; Directory.CreateDirectory(simdir); string path = Environment.GetEnvironmentVariable("PATH"); Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path, EnvironmentVariableTarget.Process); Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process); ProcessStartInfo exestart = new ProcessStartInfo(); exestart.FileName = await exepath; exestart.Arguments = extra; exestart.WorkingDirectory = simdir; exestart.WindowStyle = ProcessWindowStyle.Minimized; exestart.UseShellExecute = true; Process.Start(exestart); } try { for (int a = (int)max; a >= 0; a--) { var mav = new MAVLinkInterface(); var client = new Comms.TcpSerial(); client.client = new TcpClient("127.0.0.1", 5760 + (10 * (a))); mav.BaseStream = client; //SITLSEND = new UdpClient("127.0.0.1", 5501); Thread.Sleep(200); MainV2.instance.doConnect(mav, "preset", "5760"); try { mav.GetParam("SYSID_THISMAV"); mav.setParam("SYSID_THISMAV", a + 1, true); mav.GetParam("FRAME_CLASS"); mav.setParam("FRAME_CLASS", 1, true); } catch { } MainV2.Comports.Add(mav); } return; } catch { CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR); return; } }
private void start_Terminal(bool px4) { setcomport(); try { if (MainV2.comPort != null && MainV2.comPort.BaseStream != null && MainV2.comPort.BaseStream.IsOpen) { MainV2.comPort.BaseStream.Close(); } if (comPort.IsOpen) { Console.WriteLine("Terminal Start - Close Port"); threadrun = false; // if (DialogResult.Cancel == CustomMessageBox.Show("The port is open\n Continue?", "Continue", MessageBoxButtons.YesNo)) { // return; } comPort.Close(); // allow things to cleanup System.Threading.Thread.Sleep(400); } comPort.ReadBufferSize = 1024 * 1024 * 4; comPort.PortName = MainV2.comPortName; // test moving baud rate line comPort.BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text); if (px4) { TXT_terminal.AppendText("Rebooting " + MainV2.comPortName + " at " + comPort.BaudRate + "\n"); // keep it local using (MAVLinkInterface mine = new MAVLinkInterface()) { mine.BaseStream.PortName = MainV2.comPortName; mine.BaseStream.BaudRate = comPort.BaudRate; mine.giveComport = true; mine.BaseStream.Open(); // check if we are a mavlink stream byte[] buffer = mine.readPacket(); if (buffer.Length > 0) { log.Info("got packet - sending reboot via mavlink"); TXT_terminal.AppendText("Via Mavlink\n"); mine.doReboot(false); try { mine.BaseStream.Close(); } catch { } } else { log.Info("no packet - sending reboot via console"); TXT_terminal.AppendText("Via Console\n"); try { mine.BaseStream.Write("reboot\r"); mine.BaseStream.Write("exit\rreboot\r"); } catch { } try { mine.BaseStream.Close(); } catch { } } } TXT_terminal.AppendText("Waiting for reboot\n"); // wait 7 seconds for px4 reboot log.Info("waiting for reboot"); DateTime deadline = DateTime.Now.AddSeconds(9); while (DateTime.Now < deadline) { System.Threading.Thread.Sleep(500); Application.DoEvents(); } int a = 0; while (a < 5) { try { if (!comPort.IsOpen) { comPort.Open(); } } catch { } System.Threading.Thread.Sleep(200); a++; } } else { log.Info("About to open " + comPort.PortName); comPort.Open(); log.Info("toggle dtr"); comPort.toggleDTR(); } try { comPort.DiscardInBuffer(); } catch { } Console.WriteLine("Terminal_Load run " + threadrun + " " + comPort.IsOpen); BUT_disconnect.Enabled = true; System.Threading.Thread t11 = new System.Threading.Thread(delegate() { threadrun = true; Console.WriteLine("Terminal thread start run run " + threadrun + " " + comPort.IsOpen); try { comPort.Write("\r"); } catch { } // 10 sec waitandsleep(10000); Console.WriteLine("Terminal thread 1 run " + threadrun + " " + comPort.IsOpen); // 100 ms readandsleep(100); Console.WriteLine("Terminal thread 2 run " + threadrun + " " + comPort.IsOpen); try { if (!inlogview && comPort.IsOpen) { comPort.Write("\n\n\n"); } // 1 secs if (!inlogview && comPort.IsOpen) { readandsleep(1000); } if (!inlogview && comPort.IsOpen) { comPort.Write("\r\r\r?\r"); } } catch (Exception ex) { Console.WriteLine("Terminal thread 3 " + ex.ToString()); ChangeConnectStatus(false); threadrun = false; return; } Console.WriteLine("Terminal thread 3 run " + threadrun + " " + comPort.IsOpen); while (threadrun) { try { System.Threading.Thread.Sleep(10); if (!threadrun) { break; } if (this.Disposing) { break; } if (inlogview) { continue; } if (!comPort.IsOpen) { Console.WriteLine("Comport Closed"); ChangeConnectStatus(false); break; } if (comPort.BytesToRead > 0) { comPort_DataReceived((object)null, (SerialDataReceivedEventArgs)null); } } catch (Exception ex) { Console.WriteLine("Terminal thread 4 " + ex.ToString()); } } threadrun = false; try { comPort.DtrEnable = false; } catch { } try { Console.WriteLine("term thread close run " + threadrun + " " + comPort.IsOpen); ChangeConnectStatus(false); comPort.Close(); } catch { } Console.WriteLine("Comport thread close run " + threadrun); }); t11.IsBackground = true; t11.Name = "Terminal serial thread"; t11.Start(); // doesnt seem to work on mac //comPort.DataReceived += new SerialDataReceivedEventHandler(comPort_DataReceived); if (this.IsDisposed || this.Disposing) { return; } TXT_terminal.AppendText("Opened com port\r\n"); inputStartPos = TXT_terminal.SelectionStart; } catch (Exception ex) { log.Error(ex); TXT_terminal.AppendText("Cant open serial port\r\n"); return; } TXT_terminal.Focus(); }
void processbg(string file) { if (!File.Exists(file + ".jpg")) { LogMap.MapLogs(new string[] { file }); } var loginfo = new loginfo(); loginfo.fullname = file; loginfo.Size = new FileInfo(file).Length; if (File.Exists(file + ".jpg")) { loginfo.img = new Bitmap(file + ".jpg"); } if (file.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) { try { mine.logplaybackfile = new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; } mine.logreadmode = true; mine.getHeartBeat(); loginfo.Date = mine.lastlogread; loginfo.Aircraft = mine.sysidcurrent; var start = mine.lastlogread; try { if (mine.logplaybackfile.BaseStream.Length > MaxPlaybackLogSize) { mine.logplaybackfile.BaseStream.Seek(-100000, SeekOrigin.End); } else { mine.logplaybackfile.BaseStream.Seek(0, SeekOrigin.Begin); } } catch { } var end = mine.lastlogread; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { mine.readPacket(); if (mine.lastlogread > end) { end = mine.lastlogread; } } loginfo.Duration = (end - start).ToString(); } } logs.Add(loginfo); }
static void doread(object o) { lock (runlock) { run++; running++; } MAVLinkInterface port = (MAVLinkInterface)o; try { port.BaseStream.Open(); int baud = 0; redo: if (run == 0) { return; } DateTime deadline = DateTime.Now.AddSeconds(5); while (DateTime.Now < deadline) { Console.WriteLine("Scan port {0} at {1}", port.BaseStream.PortName, port.BaseStream.BaudRate); byte[] packet = new byte[0]; try { packet = port.readPacket(); } catch { } if (packet.Length > 0) { port.BaseStream.Close(); Console.WriteLine("Found Mavlink on port {0} at {1}", port.BaseStream.PortName, port.BaseStream.BaudRate); foundport = true; portinterface = port.BaseStream; if (CommsSerialScan.connect) { MainV2.comPort.BaseStream = port.BaseStream; doconnect(); } lock (runlock) { running--; } return; } if (foundport) { break; } } if (!foundport && port.BaseStream.BaudRate > 0) { baud++; if (baud < bauds.Length) { port.BaseStream.BaudRate = bauds[baud]; goto redo; } } try { port.BaseStream.Close(); } catch { } } catch (Exception ex) { Console.WriteLine(ex.ToString()); } finally { lock (runlock) { running--; } } Console.WriteLine("Scan port {0} Finished!!", port.BaseStream.PortName); }
public static void tlog(string logfile) { List <MLArray> mlList = new List <MLArray>(); Hashtable datappl = new Hashtable(); using (MAVLinkInterface mine = new MAVLinkInterface()) { try { mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch { CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; } mine.logreadmode = true; mine.MAV.packets.Initialize(); // clear while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); object data = mine.GetPacket(packet); if (data == null) { continue; } if (data is MAVLink.mavlink_heartbeat_t) { if (((MAVLink.mavlink_heartbeat_t)data).type == (byte)MAVLink.MAV_TYPE.GCS) { continue; } } Type test = data.GetType(); DateTime time = mine.lastlogread; double matlabtime = GetMatLabSerialDate(time); try { foreach (var field in test.GetFields()) { // field.Name has the field's name. object fieldValue = field.GetValue(data); // Get value if (field.FieldType.IsArray) { } else { if (!datappl.ContainsKey(field.Name + "_" + field.DeclaringType.Name)) { datappl[field.Name + "_" + field.DeclaringType.Name] = new List <double[]>(); } List <double[]> list = ((List <double[]>)datappl[field.Name + "_" + field.DeclaringType.Name]); object value = fieldValue; if (value.GetType() == typeof(Single)) { list.Add(new double[] { matlabtime, (double)(Single)field.GetValue(data) }); } else if (value.GetType() == typeof(short)) { list.Add(new double[] { matlabtime, (double)(short)field.GetValue(data) }); } else if (value.GetType() == typeof(ushort)) { list.Add(new double[] { matlabtime, (double)(ushort)field.GetValue(data) }); } else if (value.GetType() == typeof(byte)) { list.Add(new double[] { matlabtime, (double)(byte)field.GetValue(data) }); } else if (value.GetType() == typeof(sbyte)) { list.Add(new double[] { matlabtime, (double)(sbyte)field.GetValue(data) }); } else if (value.GetType() == typeof(Int32)) { list.Add(new double[] { matlabtime, (double)(Int32)field.GetValue(data) }); } else if (value.GetType() == typeof(UInt32)) { list.Add(new double[] { matlabtime, (double)(UInt32)field.GetValue(data) }); } else if (value.GetType() == typeof(ulong)) { list.Add(new double[] { matlabtime, (double)(ulong)field.GetValue(data) }); } else if (value.GetType() == typeof(long)) { list.Add(new double[] { matlabtime, (double)(long)field.GetValue(data) }); } else if (value.GetType() == typeof(double)) { list.Add(new double[] { matlabtime, (double)(double)field.GetValue(data) }); } else { Console.WriteLine("Unknown data type"); } } } } catch { } } mine.logreadmode = false; mine.logplaybackfile.Close(); mine.logplaybackfile = null; } foreach (string item in datappl.Keys) { double[][] temp = ((List <double[]>)datappl[item]).ToArray(); MLArray dbarray = new MLDouble(item.Replace(" ", "_"), temp); mlList.Add(dbarray); } try { MatFileWriter mfw = new MatFileWriter(logfile + ".mat", mlList, true); } catch (Exception err) { MessageBox.Show("There was an error when creating the MAT-file: \n" + err.ToString(), "MAT-File Creation Error!", MessageBoxButtons.OK, MessageBoxIcon.Exclamation); return; } }
public static void MapLogs(string[] logs) { foreach (var logfile in logs) { if (File.Exists(logfile + ".jpg")) { continue; } double minx = 99999; double maxx = -99999; double miny = 99999; double maxy = -99999; bool sitl = false; Dictionary <int, List <PointLatLngAlt> > loc_list = new Dictionary <int, List <PointLatLngAlt> >(); try { if (logfile.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) using ( mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) ) { mine.logreadmode = true; mine.speechenabled = false; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { MAVLink.MAVLinkMessage packet = mine.readPacket(); if (packet.Length < 5) { continue; } if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.SIM_STATE || packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.SIMSTATE) { sitl = true; } if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT) { var loc = packet.ToStructure <MAVLink.mavlink_global_position_int_t>(); if (loc.lat == 0 || loc.lon == 0) { continue; } var id = MAVList.GetID(packet.sysid, packet.compid); if (!loc_list.ContainsKey(id)) { loc_list[id] = new List <PointLatLngAlt>(); } loc_list[id].Add(new PointLatLngAlt(loc.lat / 10000000.0f, loc.lon / 10000000.0f)); minx = Math.Min(minx, loc.lon / 10000000.0f); maxx = Math.Max(maxx, loc.lon / 10000000.0f); miny = Math.Min(miny, loc.lat / 10000000.0f); maxy = Math.Max(maxy, loc.lat / 10000000.0f); } } } } else if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log")) { bool bin = logfile.ToLower().EndsWith(".bin"); BinaryLog binlog = new BinaryLog(); DFLog dflog = new DFLog(); using (var st = File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) { using (StreamReader sr = new StreamReader(st)) { loc_list[0] = new List <PointLatLngAlt>(); while (sr.BaseStream.Position < sr.BaseStream.Length) { string line = ""; if (bin) { line = binlog.ReadMessage(sr.BaseStream); } else { line = sr.ReadLine(); } if (line.StartsWith("FMT")) { dflog.FMTLine(line); } else if (line.StartsWith("GPS")) { var item = dflog.GetDFItemFromLine(line, 0); if (!dflog.logformat.ContainsKey("GPS")) { continue; } var status = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Status")]); var lat = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lat")]); var lon = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lng")]); if (lat == 0 || lon == 0 || status < 3) { continue; } loc_list[0].Add(new PointLatLngAlt(lat, lon)); minx = Math.Min(minx, lon); maxx = Math.Max(maxx, lon); miny = Math.Min(miny, lat); maxy = Math.Max(maxy, lat); } } } } } if (loc_list.Count > 0 && loc_list.First().Value.Count > 10) { // add a bit of buffer var area = RectLatLng.FromLTRB(minx - 0.001, maxy + 0.001, maxx + 0.001, miny - 0.001); var map = GetMap(area); var grap = Graphics.FromImage(map); if (sitl) { AddTextToMap(grap, "SITL"); } Color[] colours = { Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo, Color.Violet, Color.Pink }; int a = 0; foreach (var locs in loc_list.Values) { PointF lastpoint = new PointF(); var pen = new Pen(colours[a % (colours.Length - 1)]); foreach (var loc in locs) { PointF newpoint = GetPixel(area, loc, map.Size); if (!lastpoint.IsEmpty) { grap.DrawLine(pen, lastpoint, newpoint); } lastpoint = newpoint; } a++; } map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg); map.Dispose(); map = null; } else { DoTextMap(logfile + ".jpg", "No gps data"); } } catch (Exception ex) { if (ex.ToString().Contains("Mavlink 0.9")) { DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9"); } continue; } } }
public MAVFtp(MAVLinkInterface mavint, byte sysid, byte compid) { _mavint = mavint; _sysid = sysid; _compid = compid; }
public static void SortLogs(string[] logs) { foreach (var logfile in logs) { FileInfo info = new FileInfo(logfile); if (info.Length <= 1024) { try { string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "SMALL" + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) { Directory.CreateDirectory(destdir); } File.Move(logfile, destdir + Path.GetFileName(logfile)); File.Move(logfile.Replace(".tlog", ".rlog"), destdir + Path.GetFileName(logfile).Replace(".tlog", ".rlog")); } catch { } continue; } MAVLinkInterface mine = new MAVLinkInterface(); try { using (mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))) { mine.logreadmode = true; byte[] hbpacket = mine.getHeartBeat(); if (hbpacket.Length == 0) { continue; } MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket); mine.logreadmode = false; mine.logplaybackfile.Close(); string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) { Directory.CreateDirectory(destdir); } File.Move(logfile, destdir + Path.GetFileName(logfile)); try { File.Move(logfile.Replace(".tlog", ".rlog"), destdir + Path.GetFileName(logfile).Replace(".tlog", ".rlog")); } catch { } } } catch { continue; } } }
void processbg(string file) { a++; Loading.ShowLoading(a + "/" + files.Count + " " + file, this); if (!File.Exists(file + ".jpg")) { LogMap.MapLogs(new string[] { file }); } var loginfo = new loginfo(); loginfo.fullname = file; try { // file not found exception even though it passes the exists check above. loginfo.Size = new FileInfo(file).Length; } catch { } if (File.Exists(file + ".jpg")) { loginfo.imgfile = file + ".jpg"; } if (file.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) { try { mine.logplaybackfile = new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; } mine.logreadmode = true; mine.speechenabled = false; // file is to small if (mine.logplaybackfile.BaseStream.Length < 1024 * 4) { return; } mine.getHeartBeat(); loginfo.Date = mine.lastlogread; loginfo.Aircraft = mine.sysidcurrent; loginfo.Frame = mine.MAV.aptype.ToString(); var start = mine.lastlogread; try { mine.logplaybackfile.BaseStream.Seek(0, SeekOrigin.Begin); } catch { } var end = mine.lastlogread; var length = mine.logplaybackfile.BaseStream.Length; var a = 0; // abandon last 100 bytes while (mine.logplaybackfile.BaseStream.Position < (length - 100)) { var packet = mine.readPacket(); // gcs if (packet.sysid == 255) { continue; } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAMERA_FEEDBACK) { loginfo.CamMSG++; } if (a % 10 == 0) { mine.MAV.cs.UpdateCurrentSettings(null, true, mine); } a++; if (mine.lastlogread > end) { end = mine.lastlogread; } } loginfo.Home = mine.MAV.cs.Location; loginfo.TimeInAir = mine.MAV.cs.timeInAir; loginfo.DistTraveled = mine.MAV.cs.distTraveled; loginfo.Duration = (end - start).ToString(); } } else if (file.ToLower().EndsWith(".bin") || file.ToLower().EndsWith(".log")) { using (CollectionBuffer colbuf = new CollectionBuffer(File.OpenRead(file))) { PointLatLngAlt lastpos = null; DateTime start = DateTime.MinValue; DateTime end = DateTime.MinValue; DateTime tia = DateTime.MinValue; // set time in air/home/distancetraveled foreach (var dfItem in colbuf.GetEnumeratorType("GPS")) { if (dfItem["Status"] != null) { var status = int.Parse(dfItem["Status"]); if (status >= 3) { var pos = new PointLatLngAlt( double.Parse(dfItem["Lat"], CultureInfo.InvariantCulture), double.Parse(dfItem["Lng"], CultureInfo.InvariantCulture), double.Parse(dfItem["Alt"], CultureInfo.InvariantCulture)); if (lastpos == null) { lastpos = pos; } if (start == DateTime.MinValue) { loginfo.Date = dfItem.time; start = dfItem.time; } end = dfItem.time; // add distance loginfo.DistTraveled += (float)lastpos.GetDistance(pos); // set home if (loginfo.Home == null) { loginfo.Home = pos; } if (dfItem.time > tia.AddSeconds(1)) { // ground speed > 0.2 or alt > homelat+2 if (double.Parse(dfItem["Spd"], CultureInfo.InvariantCulture) > 0.2 || pos.Alt > (loginfo.Home.Alt + 2)) { loginfo.TimeInAir++; } tia = dfItem.time; } } } } loginfo.Duration = (end - start).ToString(); loginfo.CamMSG = colbuf.GetEnumeratorType("CAM").Count(); loginfo.Aircraft = 0; //colbuf.dflog.param[""]; loginfo.Frame = "Unknown"; //mine.MAV.aptype.ToString(); } } logs.Add(loginfo); }
/// <summary> /// Return a list of all gps messages with there timestamp from the log /// </summary> /// <param name="fn"></param> /// <returns></returns> private Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn) { Dictionary<long, VehicleLocation> vehiclePositionList = new Dictionary<long, VehicleLocation>(); // Telemetry Log if (fn.ToLower().EndsWith("tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) { mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read)); mine.logreadmode = true; CurrentState cs = new CurrentState(); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { MAVLink.MAVLinkMessage packet = mine.readPacket(); cs.datetime = mine.lastlogread; cs.UpdateCurrentSettings(null, true, mine); // check for valid lock if (!(cs.gpsstatus >=3 || cs.gpsstatus2 >= 3)) continue; VehicleLocation location = new VehicleLocation(); location.Time = cs.datetime; location.Lat = cs.lat; location.Lon = cs.lng; location.RelAlt = cs.alt; location.AltAMSL = cs.altasl; location.Roll = cs.roll; location.Pitch = cs.pitch; location.Yaw = cs.yaw; location.SAlt = cs.sonarrange; vehiclePositionList[ToMilliseconds(location.Time)] = location; // 4 5 7 Console.Write((mine.logplaybackfile.BaseStream.Position*100/ mine.logplaybackfile.BaseStream.Length) + " \r"); } mine.logplaybackfile.Close(); } } // DataFlash Log else { using (var sr = new CollectionBuffer(File.OpenRead(fn))) { // Will hold the last seen Attitude information in order to incorporate them into the GPS Info float currentYaw = 0f; float currentRoll = 0f; float currentPitch = 0f; float currentSAlt = 0f; foreach (var item in sr.GetEnumeratorType(new string[] { "GPS", "GPS2", "ATT", "CTUN", "RFND" })) { // Look for GPS Messages. However GPS Messages do not have Roll, Pitch and Yaw // So we have to look for one ATT message after having read a GPS one var gpstouse = UseGpsorGPS2(); if (item.msgtype == gpstouse) { if (!sr.dflog.logformat.ContainsKey(gpstouse)) continue; int latindex = sr.dflog.FindMessageOffset(gpstouse, "Lat"); int lngindex = sr.dflog.FindMessageOffset(gpstouse, "Lng"); int altindex = sr.dflog.FindMessageOffset(gpstouse, "Alt"); int statusindex = sr.dflog.FindMessageOffset(gpstouse, "Status"); int raltindex = sr.dflog.FindMessageOffset(gpstouse, "RAlt"); if (raltindex == -1) raltindex = sr.dflog.FindMessageOffset(gpstouse, "RelAlt"); VehicleLocation location = new VehicleLocation(); try { location.Time = item.time; if (statusindex != -1) { // check for valid lock if (double.Parse(item.items[statusindex], CultureInfo.InvariantCulture) < 3) continue; } if (latindex != -1) location.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture); if (lngindex != -1) location.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture); if (raltindex != -1) location.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture); if (altindex != -1) location.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture); location.Roll = currentRoll; location.Pitch = currentPitch; location.Yaw = currentYaw; location.SAlt = currentSAlt; long millis = ToMilliseconds(location.Time); //System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis + " GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + " TimeMS = " + getValueFromStringArray(gpsLineValues, timepos)); if (!vehiclePositionList.ContainsKey(millis) && (location.Time != DateTime.MinValue)) vehiclePositionList[millis] = location; } catch { Console.WriteLine("Bad "+gpstouse+" Line"); } } else if (item.msgtype == "ATT") { int Rindex = sr.dflog.FindMessageOffset("ATT", "Roll"); int Pindex = sr.dflog.FindMessageOffset("ATT", "Pitch"); int Yindex = sr.dflog.FindMessageOffset("ATT", "Yaw"); if (Rindex != -1) currentRoll = float.Parse(item.items[Rindex], CultureInfo.InvariantCulture); if (Pindex != -1) currentPitch = float.Parse(item.items[Pindex], CultureInfo.InvariantCulture); if (Yindex != -1) currentYaw = float.Parse(item.items[Yindex], CultureInfo.InvariantCulture); } else if (item.msgtype == "CTUN") { int SAltindex = sr.dflog.FindMessageOffset("CTUN", "SAlt"); if (SAltindex != -1) { currentSAlt = float.Parse(item.items[SAltindex], CultureInfo.InvariantCulture); } } else if (item.msgtype == "RFND") { int SAltindex = sr.dflog.FindMessageOffset("RFND", "Dist1"); if (SAltindex != -1) { currentSAlt = float.Parse(item.items[SAltindex], CultureInfo.InvariantCulture); } } } } } return vehiclePositionList; }
public MAVLinkSerialPort(MAVLinkInterface mavint, int port) : this(mavint, (MAVLink.SERIAL_CONTROL_DEV)port) { }
public static void ScanAccel() { string[] files = Directory.GetFiles(MainV2.LogDir, "*.tlog", SearchOption.AllDirectories); List<string> badfiles = new List<string>(); foreach (var file in files) { bool board = false; Console.WriteLine(file); MAVLinkInterface mavi = new MAVLinkInterface(); using (mavi.logplaybackfile = new BinaryReader(File.OpenRead(file))) { mavi.logreadmode = true; try { while (mavi.logplaybackfile.BaseStream.Position < mavi.logplaybackfile.BaseStream.Length) { byte[] packet = mavi.readPacket(); if (packet.Length == 0) break; var objectds = mavi.DebugPacket(packet, false); if (objectds is MAVLink.mavlink_param_value_t) { MAVLink.mavlink_param_value_t param = (MAVLink.mavlink_param_value_t)objectds; if (ASCIIEncoding.ASCII.GetString(param.param_id).Contains("INS_PRODUCT_ID")) { if (param.param_value == 0 || param.param_value == 5) { board = true; } else { break; } } } if (objectds is MAVLink.mavlink_raw_imu_t) { MAVLink.mavlink_raw_imu_t rawimu = (MAVLink.mavlink_raw_imu_t)objectds; if (board && Math.Abs(rawimu.xacc) > 2000 && Math.Abs(rawimu.yacc) > 2000 && Math.Abs(rawimu.zacc) > 2000) { //CustomMessageBox.Show("Bad Log " + file); badfiles.Add(file); break; } } } } catch { } } } if (badfiles.Count > 0) { FileStream fs = File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + "SearchResults.zip", FileMode.Create); ZipOutputStream zipStream = new ZipOutputStream(fs); zipStream.SetLevel(9); //0-9, 9 being the highest level of compression zipStream.UseZip64 = UseZip64.Off; // older zipfile foreach (var file in badfiles) { // entry 2 string entryName = ZipEntry.CleanName(Path.GetFileName(file)); // Removes drive from name and fixes slash direction ZipEntry newEntry = new ZipEntry(entryName); newEntry.DateTime = DateTime.Now; zipStream.PutNextEntry(newEntry); // Zip the file in buffered chunks // the "using" will close the stream even if an exception occurs byte[] buffer = new byte[4096]; using (FileStream streamReader = File.OpenRead(file)) { StreamUtils.Copy(streamReader, zipStream, buffer); } zipStream.CloseEntry(); } zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream zipStream.Close(); CustomMessageBox.Show("Added " + badfiles.Count + " logs to " + MainV2.LogDir + Path.DirectorySeparatorChar + "SearchResults.zip\n Please send this file to Craig Elder <*****@*****.**>"); } else { CustomMessageBox.Show("No Bad Logs Found"); } }
public static void SortLogs(string[] logs) { foreach (var logfile in logs) { FileInfo info = new FileInfo(logfile); // delete 0 size files if (info.Length == 0) { try { File.Delete(logfile); } catch { } continue; } // move small logs - most likerly invalid if (info.Length <= 1024) { try { string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "SMALL" + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) { Directory.CreateDirectory(destdir); } log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile)); movefileusingmask(logfile, destdir); } catch { } continue; } try { using (MAVLinkInterface mine = new MAVLinkInterface()) using ( mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))) { mine.logreadmode = true; var midpoint = mine.logplaybackfile.BaseStream.Length / 2; mine.logplaybackfile.BaseStream.Seek(midpoint, SeekOrigin.Begin); byte[] hbpacket = mine.getHeartBeat(); byte[] hbpacket1 = mine.getHeartBeat(); byte[] hbpacket2 = mine.getHeartBeat(); byte[] hbpacket3 = mine.getHeartBeat(); if (hbpacket.Length == 0 && hbpacket1.Length == 0 && hbpacket2.Length == 0 && hbpacket3.Length == 0) { mine.logreadmode = false; mine.logplaybackfile.Close(); if (!Directory.Exists(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD")) { Directory.CreateDirectory(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD"); } log.Info("Move log bad " + logfile + " to " + Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar + Path.GetFileName(logfile)); movefileusingmask(logfile, Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar); continue; } if (hbpacket.Length != 0) { MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket); } if (hbpacket1.Length != 0) { MAVLink.mavlink_heartbeat_t hb1 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket1); } if (hbpacket2.Length != 0) { MAVLink.mavlink_heartbeat_t hb2 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket2); } if (hbpacket3.Length != 0) { MAVLink.mavlink_heartbeat_t hb3 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket3); } // find most appropriate if (mine.MAVlist.Count > 1) { foreach (var mav in mine.MAVlist.GetMAVStates()) { if (mav.aptype == MAVLink.MAV_TYPE.ANTENNA_TRACKER) { continue; } if (mav.aptype == MAVLink.MAV_TYPE.GCS) { continue; } mine.sysidcurrent = mav.sysid; mine.compidcurrent = mav.compid; } } mine.logreadmode = false; mine.logplaybackfile.Close(); string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar + mine.MAV.sysid + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) { Directory.CreateDirectory(destdir); } movefileusingmask(logfile, destdir); } } catch { continue; } } }
protected override bool ProcessCmdKey(ref Message msg, Keys keyData) { if (keyData == (Keys.Control | Keys.S)) { var exepath = CheckandGetSITLImage("ArduCopter.elf"); var model = "+"; var config = GetDefaultConfig(model); var max = 10; for (int a = max; a >= 0; a--) { var extra = ""; if (!string.IsNullOrEmpty(config)) { extra += @" --defaults """ + config + @""" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 "; } var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4); if (max == a) { extra += String.Format( " -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ", a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model); } else { extra += String.Format( " -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ", a, "--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a), a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model); } string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar; Directory.CreateDirectory(simdir); string path = Environment.GetEnvironmentVariable("PATH"); Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path, EnvironmentVariableTarget.Process); Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process); ProcessStartInfo exestart = new ProcessStartInfo(); exestart.FileName = exepath; exestart.Arguments = extra; exestart.WorkingDirectory = simdir; exestart.WindowStyle = ProcessWindowStyle.Minimized; exestart.UseShellExecute = true; Process.Start(exestart); } try { var client = new Comms.TcpSerial(); client.client = new TcpClient("127.0.0.1", 5760); MainV2.comPort.BaseStream = client; SITLSEND = new UdpClient("127.0.0.1", 5501); Thread.Sleep(200); MainV2.instance.doConnect(MainV2.comPort, "preset", "5760"); return(true); } catch { CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR); return(true); } } if (keyData == (Keys.Control | Keys.D)) { var exepath = CheckandGetSITLImage("ArduCopter.elf"); var model = "+"; var config = GetDefaultConfig(model); var max = 10; for (int a = max; a >= 0; a--) { var extra = ""; if (!string.IsNullOrEmpty(config)) { extra += @" --defaults """ + config + @""" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 "; } var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4); if (max == a) { extra += String.Format( " -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ", a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model); } else { extra += String.Format( " -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ", a, "" /*"--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a)*/, a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model); } string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar; Directory.CreateDirectory(simdir); string path = Environment.GetEnvironmentVariable("PATH"); Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path, EnvironmentVariableTarget.Process); Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process); ProcessStartInfo exestart = new ProcessStartInfo(); exestart.FileName = exepath; exestart.Arguments = extra; exestart.WorkingDirectory = simdir; exestart.WindowStyle = ProcessWindowStyle.Minimized; exestart.UseShellExecute = true; Process.Start(exestart); } try { for (int a = max; a >= 0; a--) { var mav = new MAVLinkInterface(); var client = new Comms.TcpSerial(); client.client = new TcpClient("127.0.0.1", 5760 + (10 * (a))); mav.BaseStream = client; //SITLSEND = new UdpClient("127.0.0.1", 5501); Thread.Sleep(200); MainV2.instance.doConnect(mav, "preset", "5760"); MainV2.Comports.Add(mav); } return(true); } catch { CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR); return(true); } } return(base.ProcessCmdKey(ref msg, keyData)); }
/// <summary> /// Return a list of all cam messages in a log with timestamp /// </summary> /// <param name="fn"></param> /// <returns></returns> private Dictionary<long, VehicleLocation> readCAMMsgInLog(string fn) { Dictionary<long, VehicleLocation> list = new Dictionary<long, VehicleLocation>(); // Telemetry Log if (fn.ToLower().EndsWith("tlog")) { TXT_outputlog.AppendText("Warning: tlogs are not fully supported when using CAM Messages\n"); using (MAVLinkInterface mine = new MAVLinkInterface()) { mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read)); mine.logreadmode = true; CurrentState cs = new CurrentState(); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { MAVLink.MAVLinkMessage packet = mine.readPacket(); cs.datetime = mine.lastlogread; cs.UpdateCurrentSettings(null, true, mine); if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAMERA_FEEDBACK) { var msg = (MAVLink.mavlink_camera_feedback_t)packet.data; VehicleLocation location = new VehicleLocation(); location.Time = FromUTCTimeMilliseconds((long)(msg.time_usec / 1000));// cs.datetime; location.Lat = msg.lat; location.Lon = msg.lng; location.RelAlt = msg.alt_rel; location.AltAMSL = msg.alt_msl; location.Roll = msg.roll; location.Pitch = msg.pitch; location.Yaw = msg.yaw; location.SAlt = cs.sonarrange; list[ToMilliseconds(location.Time)] = location; Console.Write((mine.logplaybackfile.BaseStream.Position*100/ mine.logplaybackfile.BaseStream.Length) + " \r"); } } mine.logplaybackfile.Close(); } } // DataFlash Log else { float currentSAlt = 0; using (var sr = new CollectionBuffer(File.OpenRead(fn))) { foreach (var item in sr.GetEnumeratorType(new string[] { "CAM", "RFND" })) { if (item.msgtype == "CAM") { int latindex = sr.dflog.FindMessageOffset("CAM", "Lat"); int lngindex = sr.dflog.FindMessageOffset("CAM", "Lng"); int altindex = sr.dflog.FindMessageOffset("CAM", "Alt"); int raltindex = sr.dflog.FindMessageOffset("CAM", "RelAlt"); int rindex = sr.dflog.FindMessageOffset("CAM", "Roll"); int pindex = sr.dflog.FindMessageOffset("CAM", "Pitch"); int yindex = sr.dflog.FindMessageOffset("CAM", "Yaw"); int gtimeindex = sr.dflog.FindMessageOffset("CAM", "GPSTime"); int gweekindex = sr.dflog.FindMessageOffset("CAM", "GPSWeek"); VehicleLocation p = new VehicleLocation(); p.Time = GetTimeFromGps(int.Parse(item.items[gweekindex], CultureInfo.InvariantCulture), int.Parse(item.items[gtimeindex], CultureInfo.InvariantCulture)); p.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture); p.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture); p.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture); if (raltindex != -1) p.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture); p.Pitch = float.Parse(item.items[pindex], CultureInfo.InvariantCulture); p.Roll = float.Parse(item.items[rindex], CultureInfo.InvariantCulture); p.Yaw = float.Parse(item.items[yindex], CultureInfo.InvariantCulture); p.SAlt = currentSAlt; list[ToMilliseconds(p.Time)] = p; } else if (item.msgtype == "RFND") { int SAltindex = sr.dflog.FindMessageOffset("RFND", "Dist1"); if (SAltindex != -1) { currentSAlt = float.Parse(item.items[SAltindex], CultureInfo.InvariantCulture); } } } } } return list; }
public void setOffsets(MAVLinkInterface mav, double x, double y, double z) { offsets[mav] = new HIL.Vector3(x, y, z); //log.Info(mav.ToString() + " " + offsets[mav].ToString()); }
void processbg(string file) { Loading.ShowLoading(file, this); if (!File.Exists(file + ".jpg")) { LogMap.MapLogs(new string[] {file}); } var loginfo = new loginfo(); loginfo.fullname = file; loginfo.Size = new FileInfo(file).Length; if (File.Exists(file + ".jpg")) { loginfo.img = new Bitmap(file + ".jpg"); } if (file.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) { try { mine.logplaybackfile = new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; } mine.logreadmode = true; mine.speechenabled = false; // file is to small if (mine.logplaybackfile.BaseStream.Length < 1024*4) return; mine.getHeartBeat(); loginfo.Date = mine.lastlogread; loginfo.Aircraft = mine.sysidcurrent; loginfo.Frame = mine.MAV.aptype.ToString(); var start = mine.lastlogread; try { mine.logplaybackfile.BaseStream.Seek(0, SeekOrigin.Begin); } catch { } var end = mine.lastlogread; var length = mine.logplaybackfile.BaseStream.Length; var a = 0; // abandon last 100 bytes while (mine.logplaybackfile.BaseStream.Position < (length-100)) { var packet = mine.readPacket(); // gcs if(packet.sysid == 255) continue; if(a % 10 == 0) mine.MAV.cs.UpdateCurrentSettings(null, true, mine); a++; if (mine.lastlogread > end) end = mine.lastlogread; } loginfo.Home = mine.MAV.cs.Location; loginfo.TimeInAir = mine.MAV.cs.timeInAir; loginfo.DistTraveled = mine.MAV.cs.distTraveled; loginfo.Duration = (end - start).ToString(); } } logs.Add(loginfo); }
/// <summary> /// Return a list of all gps messages with there timestamp from the log /// </summary> /// <param name="fn"></param> /// <returns></returns> private Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn) { Dictionary<long, VehicleLocation> vehiclePositionList = new Dictionary<long, VehicleLocation>(); // Telemetry Log if (fn.ToLower().EndsWith("tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) { mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read)); mine.logreadmode = true; mine.MAV.packets.Initialize(); // clear CurrentState cs = new CurrentState(); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); cs.datetime = mine.lastlogread; cs.UpdateCurrentSettings(null, true, mine); VehicleLocation location = new VehicleLocation(); location.Time = cs.datetime; location.Lat = cs.lat; location.Lon = cs.lng; location.RelAlt = cs.alt; location.AltAMSL = cs.altasl; location.Roll = cs.roll; location.Pitch = cs.pitch; location.Yaw = cs.yaw; vehiclePositionList[ToMilliseconds(location.Time)] = location; // 4 5 7 Console.Write((mine.logplaybackfile.BaseStream.Position*100/ mine.logplaybackfile.BaseStream.Length) + " \r"); } mine.logplaybackfile.Close(); } } // DataFlash Log else { // convert bin to log if (fn.ToLower().EndsWith("bin")) { string tempfile = Path.GetTempFileName(); Log.BinaryLog.ConvertBin(fn, tempfile); fn = tempfile; } using (StreamReader sr = new StreamReader(fn)) { // Will hold the last seen Attitude information in order to incorporate them into the GPS Info float currentYaw = 0f; float currentRoll = 0f; float currentPitch = 0f; int a = 0; while (!sr.EndOfStream) { a++; string line = sr.ReadLine(); var item = dflog.GetDFItemFromLine(line, a); // Look for GPS Messages. However GPS Messages do not have Roll, Pitch and Yaw // So we have to look for one ATT message after having read a GPS one if (item.msgtype == "GPS") { if (!dflog.logformat.ContainsKey("GPS")) continue; int latindex = dflog.FindMessageOffset("GPS", "Lat"); int lngindex = dflog.FindMessageOffset("GPS", "Lng"); int altindex = dflog.FindMessageOffset("GPS", "Alt"); int raltindex = dflog.FindMessageOffset("GPS", "RAlt"); if (raltindex == -1) raltindex = dflog.FindMessageOffset("GPS", "RelAlt"); VehicleLocation location = new VehicleLocation(); try { location.Time = dflog.GetTimeGPS(line); location.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture); location.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture); location.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture); location.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture); location.Roll = currentRoll; location.Pitch = currentPitch; location.Yaw = currentYaw; long millis = ToMilliseconds(location.Time); //System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis + " GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + " TimeMS = " + getValueFromStringArray(gpsLineValues, timepos)); if (!vehiclePositionList.ContainsKey(millis) && (location.Time != DateTime.MinValue)) vehiclePositionList[millis] = location; } catch { Console.WriteLine("Bad GPS Line"); } } else if (item.msgtype == "ATT") { int Rindex = dflog.FindMessageOffset("ATT", "Roll"); int Pindex = dflog.FindMessageOffset("ATT", "Pitch"); int Yindex = dflog.FindMessageOffset("ATT", "Yaw"); currentRoll = float.Parse(item.items[Rindex], CultureInfo.InvariantCulture); currentPitch = float.Parse(item.items[Pindex], CultureInfo.InvariantCulture); currentYaw = float.Parse(item.items[Yindex], CultureInfo.InvariantCulture); } } } } return vehiclePositionList; }
public static void ScanAccel() { string[] files = Directory.GetFiles(Settings.Instance.LogDir, "*.tlog", SearchOption.AllDirectories); List <string> badfiles = new List <string>(); foreach (var file in files) { bool board = false; Console.WriteLine(file); using (MAVLinkInterface mavi = new MAVLinkInterface()) using (mavi.logplaybackfile = new BinaryReader(File.OpenRead(file))) { mavi.logreadmode = true; try { while (mavi.logplaybackfile.BaseStream.Position < mavi.logplaybackfile.BaseStream.Length) { MAVLink.MAVLinkMessage packet = mavi.readPacket(); if (packet.Length == 0) { break; } var objectds = mavi.DebugPacket(packet, false); if (objectds is MAVLink.mavlink_param_value_t) { MAVLink.mavlink_param_value_t param = (MAVLink.mavlink_param_value_t)objectds; if (ASCIIEncoding.ASCII.GetString(param.param_id).Contains("INS_PRODUCT_ID")) { if (param.param_value == 0 || param.param_value == 5) { board = true; } else { break; } } } if (objectds is MAVLink.mavlink_raw_imu_t) { MAVLink.mavlink_raw_imu_t rawimu = (MAVLink.mavlink_raw_imu_t)objectds; if (board && Math.Abs(rawimu.xacc) > 2000 && Math.Abs(rawimu.yacc) > 2000 && Math.Abs(rawimu.zacc) > 2000) { //CustomMessageBox.Show("Bad Log " + file); badfiles.Add(file); break; } } } } catch { } } } if (badfiles.Count > 0) { FileStream fs = File.Open(Settings.Instance.LogDir + Path.DirectorySeparatorChar + "SearchResults.zip", FileMode.Create); ZipOutputStream zipStream = new ZipOutputStream(fs); zipStream.SetLevel(9); //0-9, 9 being the highest level of compression zipStream.UseZip64 = UseZip64.Off; // older zipfile foreach (var file in badfiles) { // entry 2 string entryName = ZipEntry.CleanName(Path.GetFileName(file)); // Removes drive from name and fixes slash direction ZipEntry newEntry = new ZipEntry(entryName); newEntry.DateTime = DateTime.Now; zipStream.PutNextEntry(newEntry); // Zip the file in buffered chunks // the "using" will close the stream even if an exception occurs byte[] buffer = new byte[4096]; using (FileStream streamReader = File.OpenRead(file)) { StreamUtils.Copy(streamReader, zipStream, buffer); } zipStream.CloseEntry(); } zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream zipStream.Close(); CustomMessageBox.Show("Added " + badfiles.Count + " logs to " + Settings.Instance.LogDir + Path.DirectorySeparatorChar + "SearchResults.zip\n Please send this file to Craig Elder <*****@*****.**>"); } else { CustomMessageBox.Show("No Bad Logs Found"); } }
public static void SortLogs(string[] logs) { foreach (var logfile in logs) { FileInfo info = new FileInfo(logfile); // delete 0 size files if (info.Length == 0) { try { File.Delete(logfile); } catch { } continue; } // move small logs - most likerly invalid if (info.Length <= 1024) { try { string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "SMALL" + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) { Directory.CreateDirectory(destdir); } log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile)); movefileusingmask(logfile, destdir); } catch { } continue; } try { using (MAVLinkInterface mine = new MAVLinkInterface()) using ( mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))) { mine.logreadmode = true; byte[] hbpacket = mine.getHeartBeat(); if (hbpacket.Length == 0) { mine.logreadmode = false; mine.logplaybackfile.Close(); if (!Directory.Exists(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD")) { Directory.CreateDirectory(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD"); } log.Info("Move log bad " + logfile + " to " + Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar + Path.GetFileName(logfile)); movefileusingmask(logfile, Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar); continue; } MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket); mine.logreadmode = false; mine.logplaybackfile.Close(); string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) { Directory.CreateDirectory(destdir); } movefileusingmask(logfile, destdir); } } catch { continue; } } }
public static void SortLogs(string[] logs) { foreach (var logfile in logs) { FileInfo info = new FileInfo(logfile); // delete 0 size files if (info.Length == 0) { try { File.Delete(logfile); } catch { } continue; } // move small logs - most likerly invalid if (info.Length <= 1024) { try { string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "SMALL" + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) Directory.CreateDirectory(destdir); log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile)); movefileusingmask(logfile, destdir); } catch { } continue; } MAVLinkInterface mine = new MAVLinkInterface(); try { using (mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))) { mine.logreadmode = true; byte[] hbpacket = mine.getHeartBeat(); if (hbpacket.Length == 0) { mine.logreadmode = false; mine.logplaybackfile.Close(); if (!Directory.Exists(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD")) Directory.CreateDirectory(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD"); log.Info("Move log bad " + logfile + " to " + Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar + Path.GetFileName(logfile)); movefileusingmask(logfile, Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar); continue; } MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket); mine.logreadmode = false; mine.logplaybackfile.Close(); string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) Directory.CreateDirectory(destdir); movefileusingmask(logfile, destdir); } } catch { continue; } } }
void processbg(object directory) { //this.Invoke((MethodInvoker)delegate { objectListView1.Clear(); }); string[] files1 = Directory.GetFiles(directory.ToString(), "*.tlog", SearchOption.AllDirectories); string[] files2 = Directory.GetFiles(directory.ToString(), "*.bin", SearchOption.AllDirectories); string[] files3 = Directory.GetFiles(directory.ToString(), "*.log", SearchOption.AllDirectories); List<string> files = new List<string>(); files.AddRange(files1); files.AddRange(files2); files.AddRange(files3); //objectListView1.VirtualListSize = files.Length; List<object> logs = new List<object>(); foreach (var file in files) { if (!File.Exists(file + ".jpg")) { LogMap.MapLogs(new string[] {file}); } var loginfo = new loginfo(); loginfo.fullname = file; if (File.Exists(file + ".jpg")) { loginfo.img = new Bitmap(file + ".jpg"); } if (file.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) { try { mine.logplaybackfile = new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; } mine.logreadmode = true; mine.MAV.packets.Initialize(); // clear mine.getHeartBeat(); loginfo.Date = mine.lastlogread; loginfo.Aircraft = mine.sysidcurrent; var start = mine.lastlogread; try { mine.logplaybackfile.BaseStream.Seek(-100000, SeekOrigin.End); } catch { } var end = mine.lastlogread; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { mine.readPacket(); if (mine.lastlogread > end) end = mine.lastlogread; } loginfo.Duration = (end - start).ToString(); } } objectListView1.AddObject(loginfo); logs.Add(loginfo); } /* this.Invoke((MethodInvoker)delegate { objectListView1.AddObjects(logs); }); */ }
public MAVList(MAVLinkInterface mavLinkInterface) { parent = mavLinkInterface; // add blank item hiddenlist.Add(0, new MAVState(parent, 0, 0)); }
public MAVState(MAVLinkInterface mavLinkInterface, byte sysid, byte compid) { this.parent = mavLinkInterface; this.sysid = sysid; this.compid = compid; this.packetspersecond = new Dictionary <uint, double>(byte.MaxValue); this.packetspersecondbuild = new Dictionary <uint, DateTime>(byte.MaxValue); this.lastvalidpacket = DateTime.MinValue; sendlinkid = (byte)(new Random().Next(256)); signing = false; this.param = new MAVLinkParamList(); bool queuewrite = false; this.param.PropertyChanged += (s, a) => { lock (param) { if (queuewrite == true) { return; } queuewrite = true; } new Timer((o) => { try { if (cs.uid2 == null || cs.uid2 == "" || sysid == 0) { return; } if (!Directory.Exists(Path.GetDirectoryName(ParamCachePath))) { Directory.CreateDirectory(Path.GetDirectoryName(ParamCachePath)); } lock (this.param) File.WriteAllText(ParamCachePath, param.ToJSON()); } catch (Exception e) { log.Error(e); } queuewrite = false; }, null, 2000, -1); }; this.packets = new Dictionary <uint, Queue <MAVLinkMessage> >(byte.MaxValue); this.packetsLast = new Dictionary <uint, MAVLinkMessage>(byte.MaxValue); this.aptype = 0; this.apname = 0; this.recvpacketcount = 0; this.VersionString = ""; this.SoftwareVersions = ""; this.SerialString = ""; this.FrameString = ""; if (sysid != 255 && !(compid == 0 && sysid == 0)) // && !parent.logreadmode) { this.Proximity = new Proximity(this); } camerapoints.Clear(); this.packetslost = 0f; this.packetsnotlost = 0f; this.packetlosttimer = DateTime.MinValue; cs.parent = this; }
public static void SortLogs(string[] logs) { foreach (var logfile in logs) { FileInfo info = new FileInfo(logfile); // delete 0 size files if (info.Length == 0) { try { File.Delete(logfile); } catch { } continue; } // move small logs - most likerly invalid if (info.Length <= 1024) { try { string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "SMALL" + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) Directory.CreateDirectory(destdir); log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile)); movefileusingmask(logfile, destdir); } catch { } continue; } try { using (MAVLinkInterface mine = new MAVLinkInterface()) using ( mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))) { mine.logreadmode = true; var midpoint = mine.logplaybackfile.BaseStream.Length / 2; mine.logplaybackfile.BaseStream.Seek(midpoint, SeekOrigin.Begin); MAVLink.MAVLinkMessage hbpacket = mine.getHeartBeat(); MAVLink.MAVLinkMessage hbpacket1 = mine.getHeartBeat(); MAVLink.MAVLinkMessage hbpacket2 = mine.getHeartBeat(); MAVLink.MAVLinkMessage hbpacket3 = mine.getHeartBeat(); if (hbpacket.Length == 0 && hbpacket1.Length == 0 && hbpacket2.Length == 0 && hbpacket3.Length == 0) { mine.logreadmode = false; mine.logplaybackfile.Close(); if (!Directory.Exists(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD")) Directory.CreateDirectory(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD"); log.Info("Move log bad " + logfile + " to " + Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar + Path.GetFileName(logfile)); movefileusingmask(logfile, Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar); continue; } if (hbpacket.Length != 0) { MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t) mine.DebugPacket(hbpacket); } if (hbpacket1.Length != 0) { MAVLink.mavlink_heartbeat_t hb1 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket1); } if (hbpacket2.Length != 0) { MAVLink.mavlink_heartbeat_t hb2 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket2); } if (hbpacket3.Length != 0) { MAVLink.mavlink_heartbeat_t hb3 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket3); } // find most appropriate if (mine.MAVlist.Count > 1) { foreach (var mav in mine.MAVlist.GetMAVStates()) { if (mav.aptype == MAVLink.MAV_TYPE.ANTENNA_TRACKER) continue; if (mav.aptype == MAVLink.MAV_TYPE.GCS) continue; mine.sysidcurrent = mav.sysid; mine.compidcurrent = mav.compid; } } mine.logreadmode = false; mine.logplaybackfile.Close(); string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar + mine.MAV.sysid + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) Directory.CreateDirectory(destdir); movefileusingmask(logfile, destdir); } } catch { continue; } } }
internal void processToScreen() { toolTip1.RemoveAll(); disableNumericUpDownControls(this); // process hashdefines and update display foreach (string value in MainV2.comPort.MAV.param.Keys) { if (value == null || value == "") { continue; } //System.Diagnostics.Debug.WriteLine("Doing: " + value); string name = value; Control[] text = this.Controls.Find(name, true); foreach (Control ctl in text) { try { if (ctl.GetType() == typeof(NumericUpDown)) { float numbervalue = (float)MainV2.comPort.MAV.param[value]; MAVLinkInterface.modifyParamForDisplay(true, value, ref numbervalue); NumericUpDown thisctl = ((NumericUpDown)ctl); thisctl.Maximum = 9000; thisctl.Minimum = -9000; thisctl.Value = (decimal)numbervalue; thisctl.Increment = (decimal)0.0001; if (thisctl.Name.EndsWith("_P") || thisctl.Name.EndsWith("_I") || thisctl.Name.EndsWith("_D") || thisctl.Name.EndsWith("_LOW") || thisctl.Name.EndsWith("_HIGH") || thisctl.Value == 0 || thisctl.Value.ToString("0.####", new System.Globalization.CultureInfo("en-US")).Contains(".")) { thisctl.DecimalPlaces = 4; } else { thisctl.Increment = (decimal)1; thisctl.DecimalPlaces = 1; } if (thisctl.Name.ToUpper().EndsWith("THR_RATE_IMAX")) { thisctl.Maximum = 1000; // is a pwm thisctl.Minimum = 0; } else if (thisctl.Name.EndsWith("_IMAX")) { thisctl.Maximum = 1800; thisctl.Minimum = -1800; } thisctl.Enabled = true; ThemeManager.ApplyThemeTo(thisctl); thisctl.Validated += null; if (tooltips[value] != null) { try { toolTip1.SetToolTip(ctl, ((paramsettings)tooltips[value]).desc); } catch { } } thisctl.Validated += new EventHandler(EEPROM_View_float_TextChanged); } else if (ctl.GetType() == typeof(ComboBox)) { ComboBox thisctl = ((ComboBox)ctl); thisctl.SelectedValue = (int)(float)MainV2.comPort.MAV.param[value]; thisctl.Validated += new EventHandler(ComboBox_Validated); ThemeManager.ApplyThemeTo(thisctl); } } catch { } } if (text.Length == 0) { //Console.WriteLine(name + " not found"); } } }
/// <summary> /// Recevied UDP packet, process and send required data to serial port. /// </summary> /// <param name="data">Packet</param> /// <param name="receviedbytes">Length</param> /// <param name="comPort">Com Port</param> private void RECVprocess(byte[] data, int receviedbytes, MAVLinkInterface comPort) { var sitldata = new sitl_fdm(); sitldata.timestamp = (ulong) (simtime*1.0e6); if (data[0] == 'D' && data[1] == 'A') { // Xplanes sends // 5 byte header // 1 int for the index - numbers on left of output // 8 floats - might be useful. or 0 if not var count = 5; while (count < receviedbytes) { var index = BitConverter.ToInt32(data, count); DATA[index] = new float[8]; DATA[index][0] = BitConverter.ToSingle(data, count + 1*4); ; DATA[index][1] = BitConverter.ToSingle(data, count + 2*4); ; DATA[index][2] = BitConverter.ToSingle(data, count + 3*4); ; DATA[index][3] = BitConverter.ToSingle(data, count + 4*4); ; DATA[index][4] = BitConverter.ToSingle(data, count + 5*4); ; DATA[index][5] = BitConverter.ToSingle(data, count + 6*4); ; DATA[index][6] = BitConverter.ToSingle(data, count + 7*4); ; DATA[index][7] = BitConverter.ToSingle(data, count + 8*4); ; count += 36; // 8 * float } var xplane9 = !CHK_xplane10.Checked; if (xplane9) { sitldata.pitchDeg = (DATA[18][0]); sitldata.rollDeg = (DATA[18][1]); sitldata.yawDeg = (DATA[18][2]); sitldata.pitchRate = (DATA[17][0]*rad2deg); sitldata.rollRate = (DATA[17][1]*rad2deg); sitldata.yawRate = (DATA[17][2]*rad2deg); sitldata.heading = DATA[19][2]; sitldata.speedN = -DATA[21][5]; // (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad)); sitldata.speedE = DATA[21][3]; // (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad)); sitldata.speedD = -DATA[21][4]; } else { sitldata.pitchDeg = (DATA[17][0]); sitldata.rollDeg = (DATA[17][1]); sitldata.yawDeg = (DATA[17][2]); sitldata.pitchRate = (DATA[16][0]*rad2deg); sitldata.rollRate = (DATA[16][1]*rad2deg); sitldata.yawRate = (DATA[16][2]*rad2deg); sitldata.heading = (DATA[18][2]); // 18-2 sitldata.speedN = -DATA[21][5]; // (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad)); sitldata.speedE = DATA[21][3]; // (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad)); sitldata.speedD = -DATA[21][4]; } sitldata.airspeed = ((DATA[3][5]*.44704)); sitldata.latitude = (DATA[20][0]); sitldata.longitude = (DATA[20][1]); sitldata.altitude = (DATA[20][2]*ft2m); //http://en.wikipedia.org/wiki/Banked_turn var dcm = new Matrix3(); dcm.from_euler(sitldata.rollDeg*deg2rad, sitldata.pitchDeg*deg2rad, sitldata.yawDeg*deg2rad); // v squared in m/s var v_sq = (float) ((DATA[3][7]*0.44704)*(DATA[3][7]*0.44704)); // rad = tas^2 / (tan(angle) * G) var turnrad = v_sq/(float) (9.808f*Math.Tan(sitldata.rollDeg*deg2rad)); var gload = (float) (1/Math.Cos(sitldata.rollDeg*deg2rad)); // calculated Gs // a = v^2/r var centripaccel = v_sq/turnrad; var accel_body = dcm.transposed()*(new Vector3(0, 0, -9.808)); var centrip_accel = new Vector3(0, -centripaccel*Math.Cos(sitldata.rollDeg*deg2rad), centripaccel*Math.Sin(sitldata.rollDeg*deg2rad)); var pitchpart = centrip_accel.y*Math.Cos(sitldata.pitchDeg*deg2rad); //centrip_accel.x += pitchpart; //centrip_accel.y += pitchpart; //centrip_accel.y = -centripaccel; accel_body -= centrip_accel; var velocitydelta = dcm.transposed()* (new Vector3((sitldata_old.speedN - sitldata.speedN), (sitldata_old.speedE - sitldata.speedE), (sitldata_old.speedD - sitldata.speedD))); // v = d/t a = v/t add linear accel - t is 50 hz //accel_body.x += -velocitydelta.x / 0.02; //Vector3 velocity = dcm.transposed() * (new Vector3((sitldata.speedN), (sitldata.speedE), (sitldata.speedD))); //Console.WriteLine("vel " + velocity.ToString()); //Console.WriteLine("ved " + velocitydelta.ToString()); // a = dv / dt // 50 hz = 0.02sec var accel_mine_body = dcm.transposed()* (new Vector3((sitldata_old.speedN - sitldata.speedN)/0.02, (sitldata_old.speedE - sitldata.speedE)/0.02, (sitldata_old.speedD - sitldata.speedD)/0.02)); // Console.WriteLine("G"+accel_body.ToString()); // Console.WriteLine("M"+accel_mine_body.ToString()); //sitldata.pitchRate = 0; //sitldata.rollRate = 0; //sitldata.yawRate = 0; sitldata.xAccel = accel_body.x; // DATA[4][5] * 1; sitldata.yAccel = accel_body.y; // DATA[4][6] * 1; sitldata.zAccel = accel_body.z; // (0 - DATA[4][4]) * 9.808; //sitldata.xAccel = DATA[4][5] *9.808; //sitldata.yAccel = DATA[4][6] *9.808; //sitldata.zAccel = -DATA[4][4] *9.808; } else if (receviedbytes == 0x64) // FG binary udp { //FlightGear /* fgIMUData imudata2 = data.ByteArrayToStructureBigEndian<fgIMUData>(0); if (imudata2.magic != 0x4c56414d) return; if (imudata2.latitude == 0) return; chkSensor.Checked = true; imu.time_usec = ((ulong)DateTime.Now.ToBinary()); imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2)); imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293)); imu.xmag = 0; imu.yacc = ((Int16)(imudata2.accelY * 9808 / 32.2)); imu.ygyro = ((Int16)(imudata2.ratePitch * 17.453293)); imu.ymag = 0; imu.zacc = ((Int16)(imudata2.accelZ * 9808 / 32.2)); // + 1000 imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293)); imu.zmag = 0; 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); //FileStream stream = File.OpenWrite("fgdata.txt"); //stream.Write(data, 0, receviedbytes); //stream.Close(); */ } else if (receviedbytes == 662 || receviedbytes == 658) // 658 = 3.83 662 = 3.91 { /* // Model data in body frame coordinates (X=Right, Y=Front, Z=Up) public float Model_fVel_Body_X; public float Model_fVel_Body_Y; public float Model_fVel_Body_Z; // m/s Model velocity in body coordinates public float Model_fAngVel_Body_X; public float Model_fAngVel_Body_Y; public float Model_fAngVel_Body_Z; // rad/s Model angular velocity in body coordinates public float Model_fAccel_Body_X; public float Model_fAccel_Body_Y; public float Model_fAccel_Body_Z; // m/s/s Model acceleration in body coordinates */ var aeroin_last = aeroin; aeroin = data.ByteArrayToStructure<TDataFromAeroSimRC>(0); sitldata.pitchDeg = (aeroin.Model_fPitch*rad2deg); sitldata.rollDeg = (aeroin.Model_fRoll*-1*rad2deg); sitldata.yawDeg = ((aeroin.Model_fHeading*rad2deg)); sitldata.pitchRate = aeroin.Model_fAngVel_Body_X*rad2deg; sitldata.rollRate = aeroin.Model_fAngVel_Body_Y*rad2deg; sitldata.yawRate = -aeroin.Model_fAngVel_Body_Z*rad2deg; // calc gravity var dcm = new Matrix3(); dcm.from_euler(sitldata.rollDeg*deg2rad, sitldata.pitchDeg*deg2rad, sitldata.yawDeg*deg2rad); var accel_body = dcm.transposed()*(new Vector3(0, 0, -9.8)); // -9.8 sitldata.xAccel = aeroin.Model_fAccel_Body_Y/9.808 + accel_body.x; // pitch - back forward- sitldata.yAccel = aeroin.Model_fAccel_Body_X/9.808 + accel_body.y; // roll - left right- sitldata.zAccel = -aeroin.Model_fAccel_Body_Z/9.808 + accel_body.z; // Console.WriteLine("2 {0,20} {1,20} {2,20}", aeroin.Model_fAccel_Body_X.ToString("0.000"), aeroin.Model_fAccel_Body_Y.ToString("0.000"), aeroin.Model_fAccel_Body_Z.ToString("0.000")); sitldata.altitude = aeroin.Model_fPosZ; sitldata.latitude = aeroin.Model_fLatitude; sitldata.longitude = aeroin.Model_fLongitude; sitldata.speedN = aeroin.Model_fVelY; sitldata.speedE = aeroin.Model_fVelX; sitldata.speedD = aeroin.Model_fVelZ; var xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY; var yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX; sitldata.airspeed = ((float) Math.Sqrt((yvec*yvec) + (xvec*xvec))); } else if (receviedbytes == 408) { var fdm = data.ByteArrayToStructureBigEndian<FGNetFDM>(0); lastfdmdata = fdm; sitldata.timestamp = (ulong) (simtime*1.0e6); sitldata.altitude = (fdm.altitude); sitldata.latitude = (fdm.latitude*rad2deg); sitldata.longitude = (fdm.longitude*rad2deg); sitldata.rollDeg = fdm.phi*rad2deg; sitldata.pitchDeg = fdm.theta*rad2deg; sitldata.yawDeg = fdm.psi*rad2deg; sitldata.rollRate = fdm.phidot*rad2deg; sitldata.pitchRate = fdm.thetadot*rad2deg; sitldata.yawRate = fdm.psidot*rad2deg; sitldata.speedN = fdm.v_north*ft2m; sitldata.speedE = fdm.v_east*ft2m; sitldata.speedD = fdm.v_down*ft2m; sitldata.xAccel = (fdm.A_X_pilot*9.808/32.2); // pitch sitldata.yAccel = (fdm.A_Y_pilot*9.808/32.2); // roll sitldata.zAccel = (fdm.A_Z_pilot/32.2*9.808); sitldata.airspeed = fdm.vcas*0.5144444f; // knots to m/s //Console.WriteLine("1 {0} {1} {2} {3}",(float)sitldata.rollDeg,MainV2.comPort.MAV.cs.roll,sitldata.pitchDeg,MainV2.comPort.MAV.cs.pitch); if (RAD_JSBSim.Checked) sitldata.airspeed = fdm.vcas*ft2m; // fps to m/s } else { log.Info("Bad Udp Packet " + receviedbytes); return; } if (sitldata.altitude < 0) sitldata.altitude = 0.00001; sitldata_old = sitldata; var gpsspan = DateTime.Now - lastgpsupdate; // add gps delay if (gpsspan.TotalMilliseconds >= GPS_rate) { lastgpsupdate = DateTime.Now; // save current fix = 3 sitl_fdmbuffer[gpsbufferindex%sitl_fdmbuffer.Length] = sitldata; // Console.WriteLine((gpsbufferindex % gpsbuffer.Length) + " " + ((gpsbufferindex + (gpsbuffer.Length - 1)) % gpsbuffer.Length)); // return buffer index + 5 = (3 + 5) = 8 % 6 = 2 oldgps = sitl_fdmbuffer[(gpsbufferindex + (sitl_fdmbuffer.Length - 1))%sitl_fdmbuffer.Length]; //comPort.sendPacket(oldgps); gpsbufferindex++; } var hilstate = new MAVLink.mavlink_hil_state_t(); var epochBegin = new DateTime(1980, 1, 6, 0, 0, 0, DateTimeKind.Utc); hilstate.time_usec = (ulong) ((DateTime.Now.Ticks - epochBegin.Ticks)/10); // microsec hilstate.lat = (int) (oldgps.latitude*1e7); // * 1E7 hilstate.lon = (int) (oldgps.longitude*1e7); // * 1E7 hilstate.alt = (int) (oldgps.altitude*1000); // mm // Console.WriteLine(hilstate.alt); // Console.WriteLine("{0} {1} {2}", sitldata.rollDeg.ToString("0.0"), sitldata.pitchDeg.ToString("0.0"), sitldata.yawDeg.ToString("0.0")); hilstate.pitch = (float) sitldata.pitchDeg*deg2rad; // (rad) hilstate.pitchspeed = (float) sitldata.pitchRate*deg2rad; // (rad/s) hilstate.roll = (float) sitldata.rollDeg*deg2rad; // (rad) hilstate.rollspeed = (float) sitldata.rollRate*deg2rad; // (rad/s) hilstate.yaw = (float) sitldata.yawDeg*deg2rad; // (rad) hilstate.yawspeed = (float) sitldata.yawRate*deg2rad; // (rad/s) hilstate.vx = (short) (sitldata.speedN*100); // m/s * 100 - lat hilstate.vy = (short) (sitldata.speedE*100); // m/s * 100 - long hilstate.vz = (short) (sitldata.speedD*100); // m/s * 100 - + speed down hilstate.xacc = (short) (sitldata.xAccel*101.957); // (mg) hilstate.yacc = (short) (sitldata.yAccel*101.957); // (mg) hilstate.zacc = (short) (sitldata.zAccel*101.957); // (mg) packetcount++; if (!comPort.BaseStream.IsOpen) return; if (comPort.BaseStream.BytesToWrite > 100) return; // if (packetcount % 2 == 0) // return; comPort.sendPacket(hilstate,comPort.sysidcurrent,comPort.compidcurrent); // comPort.sendPacket(oldgps); //comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t() { airspeed = (float)sitldata.airspeed } ); var pres = new MAVLink.mavlink_raw_pressure_t(); var calc = (101325*Math.Pow(1 - 2.25577*Math.Pow(10, -5)*sitldata.altitude, 5.25588)); // updated from valid gps pres.press_diff1 = (short) (int) (calc - 101325); // 0 alt is 0 pa // comPort.sendPacket(pres); }
internal void EEPROM_View_float_TextChanged(object sender, EventArgs e) { if (startup == true) { return; } float value = 0; string name = ((Control)sender).Name; // do domainupdown state check try { if (sender.GetType() == typeof(NumericUpDown)) { value = (float)((NumericUpDown)sender).Value; MAVLinkInterface.modifyParamForDisplay(false, ((Control)sender).Name, ref value); changes[name] = value; } else if (sender.GetType() == typeof(ComboBox)) { value = (int)((ComboBox)sender).SelectedValue; changes[name] = value; } ((Control)sender).BackColor = Color.Green; } catch (Exception) { ((Control)sender).BackColor = Color.Red; } try { // enable roll and pitch pairing for ac2 if (CHK_lockrollpitch.Checked) { if (name.StartsWith("RATE_") || name.StartsWith("STB_") || name.StartsWith("ACRO_")) { if (name.Contains("_RLL_")) { string newname = name.Replace("_RLL_", "_PIT_"); Control[] arr = this.Controls.Find(newname, true); changes[newname] = value; if (arr.Length > 0) { arr[0].Text = ((Control)sender).Text; arr[0].BackColor = Color.Green; } } else if (name.Contains("_PIT_")) { string newname = name.Replace("_PIT_", "_RLL_"); Control[] arr = this.Controls.Find(newname, true); changes[newname] = value; if (arr.Length > 0) { arr[0].Text = ((Control)sender).Text; arr[0].BackColor = Color.Green; } } } } // keep nav_lat and nav_lon paired if (name.Contains("NAV_LAT_")) { string newname = name.Replace("NAV_LAT_", "NAV_LON_"); Control[] arr = this.Controls.Find(newname, true); changes[newname] = value; if (arr.Length > 0) { arr[0].Text = ((Control)sender).Text; arr[0].BackColor = Color.Green; } } // keep loiter_lat and loiter_lon paired if (name.Contains("LOITER_LAT_")) { string newname = name.Replace("LOITER_LAT_", "LOITER_LON_"); Control[] arr = this.Controls.Find(newname, true); changes[newname] = value; if (arr.Length > 0) { arr[0].Text = ((Control)sender).Text; arr[0].BackColor = Color.Green; } } // keep nav_lat and nav_lon paired if (name.Contains("HLD_LAT_")) { string newname = name.Replace("HLD_LAT_", "HLD_LON_"); Control[] arr = this.Controls.Find(newname, true); changes[newname] = value; if (arr.Length > 0) { arr[0].Text = ((Control)sender).Text; arr[0].BackColor = Color.Green; } } } catch { } }
public void UploadFence(MAVLinkInterface port, Action <int, string> progress = null) { var fence = FenceToLocation(); mav_mission.upload(port, MAVLink.MAV_MISSION_TYPE.FENCE, fence, progress); }
internal void processToScreen() { toolTip1.RemoveAll(); disableNumericUpDownControls(this); // process hashdefines and update display foreach (string value in MainV2.comPort.MAV.param.Keys) { if (value == null || value == "") { continue; } var name = value; var text = Controls.Find(name, true); foreach (var ctl in text) { try { if (ctl.GetType() == typeof(NumericUpDown)) { var numbervalue = (float)MainV2.comPort.MAV.param[value]; MAVLinkInterface.modifyParamForDisplay(true, value, ref numbervalue); var thisctl = ((NumericUpDown)ctl); thisctl.Maximum = 9000; thisctl.Minimum = -9000; thisctl.Value = (decimal)numbervalue; thisctl.Increment = (decimal)0.001; if (thisctl.Name.EndsWith("_P") || thisctl.Name.EndsWith("_I") || thisctl.Name.EndsWith("_D") || thisctl.Name.EndsWith("_LOW") || thisctl.Name.EndsWith("_HIGH") || thisctl.Value == 0 || thisctl.Value.ToString("0.###", new CultureInfo("en-US")).Contains(".")) { thisctl.DecimalPlaces = 3; } else { thisctl.Increment = 1; thisctl.DecimalPlaces = 1; } if (thisctl.Name.EndsWith("_IMAX")) { thisctl.Maximum = 180; thisctl.Minimum = -180; } thisctl.Enabled = true; ThemeManager.ApplyThemeTo(thisctl); thisctl.Validated += null; if (tooltips[value] != null) { try { toolTip1.SetToolTip(ctl, ((paramsettings)tooltips[value]).desc); } catch { } } thisctl.Validated += EEPROM_View_float_TextChanged; } else if (ctl.GetType() == typeof(ComboBox)) { var thisctl = ((ComboBox)ctl); thisctl.SelectedIndex = (int)(float)MainV2.comPort.MAV.param[value]; thisctl.Validated += ComboBox_Validated; ThemeManager.ApplyThemeTo(thisctl); } } catch { } } if (text.Length == 0) { //Console.WriteLine(name + " not found"); } } }