public DebugPacket ( MAVLinkMessage datin ) : object | ||
datin | MAVLinkMessage | |
return | object |
public static string tlogToCSV(string filepath) { CurrentState.SpeedUnit = "m/s"; CurrentState.DistanceUnit = "m"; MAVLinkInterface proto = new MAVLinkInterface(); OpenFileDialog openFileDialog1 = new OpenFileDialog(); string LogFilePath; openFileDialog1.FileName = filepath; foreach (string logfile in openFileDialog1.FileNames) { using (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()); } mine.logreadmode = true; mine.MAV.packets.Initialize(); // clear StreamWriter sw = new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".csv"); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); string text = ""; mine.DebugPacket(packet, ref text, true, ","); sw.Write(mine.lastlogread.ToString("yyyy-MM-ddTHH:mm:ss.fff") + "," + text); } sw.Close(); mine.logreadmode = false; mine.logplaybackfile.Close(); mine.logplaybackfile = null; LogFilePath = (Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + (Path.GetFileNameWithoutExtension(logfile) + ".csv")); return LogFilePath; } } return null; }
/// <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); using (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; }