public static void anonymise(string logfile, string outputfile) { if (!File.Exists(logfile)) { return; } var rand = new Random(); var latrandom = rand.NextDouble() * (rand.NextDouble() * 3); // TLOG if (logfile.ToLower().EndsWith(".tlog")) { Comms.CommsFile tlogFile = new CommsFile(); tlogFile.Open(logfile); using (var stream = new CommsStream(tlogFile, tlogFile.BytesToRead)) using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read)) { MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse(true); var block = new Type[] { typeof(MAVLink.mavlink_fence_point_t), typeof(MAVLink.mavlink_simstate_t), typeof(MAVLink.mavlink_rally_point_t), typeof(MAVLink.mavlink_ahrs2_t), typeof(MAVLink.mavlink_camera_feedback_t), typeof(MAVLink.mavlink_ahrs3_t), typeof(MAVLink.mavlink_deepstall_t), typeof(MAVLink.mavlink_gps_raw_int_t), typeof(MAVLink.mavlink_global_position_int_t), typeof(MAVLink.mavlink_set_gps_global_origin_t), typeof(MAVLink.mavlink_gps_global_origin_t), typeof(MAVLink.mavlink_global_position_int_cov_t), typeof(MAVLink.mavlink_set_position_target_global_int_t), typeof(MAVLink.mavlink_hil_state_t), typeof(MAVLink.mavlink_sim_state_t), typeof(MAVLink.mavlink_hil_gps_t), typeof(MAVLink.mavlink_hil_state_quaternion_t), typeof(MAVLink.mavlink_gps2_raw_t), typeof(MAVLink.mavlink_terrain_request_t), typeof(MAVLink.mavlink_terrain_check_t), typeof(MAVLink.mavlink_terrain_report_t), typeof(MAVLink.mavlink_follow_target_t), typeof(MAVLink.mavlink_gps_input_t), typeof(MAVLink.mavlink_high_latency_t), typeof(MAVLink.mavlink_home_position_t), typeof(MAVLink.mavlink_set_home_position_t), typeof(MAVLink.mavlink_adsb_vehicle_t), typeof(MAVLink.mavlink_camera_image_captured_t), typeof(MAVLink.mavlink_uavionix_adsb_out_dynamic_t), typeof(MAVLink.mavlink_global_position_int_t), typeof(MAVLink.mavlink_set_home_position_t), typeof(MAVLink.mavlink_home_position_t), typeof(MAVLink.mavlink_set_position_target_global_int_t), typeof(MAVLink.mavlink_local_position_ned_t), typeof(MAVLink.mavlink_command_long_t), typeof(MAVLink.mavlink_mission_item_t), typeof(MAVLink.mavlink_mission_item_int_t), typeof(MAVLink.mavlink_uavionix_adsb_out_cfg_t) }; var checks = new string[] { "lat", "latitude", "lat_int", "landing_lat", "path_lat", "arc_entry_lat", "gpsLat", "gpsOffsetLat" }; while (stream.Position < stream.Length) { var packet = parse.ReadPacket(stream); if (packet == null) { continue; } var msginfo = MAVLink.MAVLINK_MESSAGE_INFOS.GetMessageInfo(packet.msgid); if (block.Contains(msginfo.type)) { bool valid = false; var oldrxtime = packet.rxtime; foreach (var check in checks) { var field = msginfo.type.GetField(check); if (field != null) { var pkt = packet.data; var value = field.GetValue(pkt); if (value is Int32) { if ((int)value != 0) { field.SetValue(pkt, (int)((int)value + latrandom * 1e7)); } packet = new MAVLink.MAVLinkMessage( parse.GenerateMAVLinkPacket20((MAVLink.MAVLINK_MSG_ID)msginfo.msgid, pkt, false, packet.sysid, packet.compid, packet.seq)); valid = true; } else if (value is Single) { if ((Single)value != 0) { field.SetValue(pkt, (Single)value + latrandom); } packet = new MAVLink.MAVLinkMessage( parse.GenerateMAVLinkPacket20((MAVLink.MAVLINK_MSG_ID)msginfo.msgid, pkt, false, packet.sysid, packet.compid, packet.seq)); valid = true; } else { continue; } } } packet.rxtime = oldrxtime; } byte[] datearray = BitConverter.GetBytes( (UInt64)((packet.rxtime.ToUniversalTime() - new DateTime(1970, 1, 1)) .TotalMilliseconds * 1000)); Array.Reverse(datearray); outfilestream.Write(datearray, 0, datearray.Length); outfilestream.Write(packet.buffer, 0, packet.Length); } } } else //LOG { using (DFLogBuffer col = new DFLogBuffer(File.OpenRead(logfile))) using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read)) { foreach (var dfItem in col.GetEnumeratorTypeAll()) { var index = col.dflog.FindMessageOffset(dfItem.msgtype, "lat"); if (index != -1) { var lat = Double.Parse(dfItem.items[index], CultureInfo.InvariantCulture); if (lat != 0) { dfItem.items[index] = (lat + latrandom).ToString(CultureInfo.InvariantCulture); } } var str = String.Join(",", dfItem.items) + "\r\n"; outfilestream.Write(ASCIIEncoding.ASCII.GetBytes(str), 0, str.Length); } } } }
public static void tlog(string logfile) { List <MLArray> mlList = new List <MLArray>(); Hashtable datappl = new Hashtable(); using (Comms.CommsFile cf = new CommsFile(logfile)) using (CommsStream cs = new CommsStream(cf, cf.BytesToRead)) { MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse(true); while (cs.Position < cs.Length) { MAVLink.MAVLinkMessage packet = parse.ReadPacket(cs); if (packet == null) { continue; } object data = packet.data; 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 = packet.rxtime; 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 { } } } 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, false); } catch (Exception err) { throw new Exception("There was an error when creating the MAT-file: \n" + err.ToString(), err); } }
private void CoTmainloop() { CoTthreadrun = true; var stream = new CommsStream(CoTStream, int.MaxValue); var lookfor = "<?xml".Select(a => (byte)a).ToArray(); var lookforend = "</event>".Select(a => (byte)a).ToArray(); while (CoTthreadrun) { try { var sb = new StringBuilder(); var buff = new byte[4096]; while (CoTthreadrun) { var by = stream.Read(buff, 0, buff.Length); var pos = 0; while (pos < by) { var start = buff.Search(lookfor, pos); var end = buff.Search(lookforend, pos); // entire message if (start >= 0 && end >= 0 && end > start) { var xml = ASCIIEncoding.ASCII.GetString(buff.Skip(start).Take(end + lookforend.Length - start).ToArray()); pos = end + lookforend.Length; ProcessCoTMessage(xml); }// start with no end else if (start >= 0 && end == -1) { var partxml = ASCIIEncoding.ASCII.GetString(buff.Skip(start).ToArray()); pos = by; sb.Append(partxml); }// no start with end else if (end >= 0) { var partxml = ASCIIEncoding.ASCII.GetString(buff.Take(end + lookforend.Length).ToArray()); sb.Append(partxml); ProcessCoTMessage(sb.ToString()); sb.Clear(); pos = end + lookforend.Length; }// no start or end else { sb.Append(ASCIIEncoding.ASCII.GetString(buff)); pos = by; } } } } catch { } finally { } } }
public static void anonymise(string logfile, string outputfile) { var latrandom = new Random().NextDouble(); //LOG using (CollectionBuffer col = new CollectionBuffer(File.OpenRead(logfile))) using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read)) { foreach (var dfItem in col.GetEnumeratorTypeAll()) { var index = col.dflog.FindMessageOffset(dfItem.msgtype, "lat"); if (index != -1) { dfItem.items[index] = (Double.Parse(dfItem.items[index], CultureInfo.InvariantCulture) + latrandom).ToString( CultureInfo.InvariantCulture); } var str = String.Join(",", dfItem.items) + "\r\n"; outfilestream.Write(ASCIIEncoding.ASCII.GetBytes(str), 0, str.Length); } } return; // TLOG Comms.CommsFile tlogFile = new CommsFile(); tlogFile.Open(logfile); using (var stream = new CommsStream(tlogFile, tlogFile.BytesToRead)) using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read)) { MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse(); double lat = 0; double lng = 0; while (stream.Position < stream.Length) { var packet = parse.ReadPacket(stream); if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT) { packet.ToStructure <MAVLink.mavlink_global_position_int_t>(); } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.POSITION_TARGET_GLOBAL_INT) { } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.SET_HOME_POSITION) { } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.HOME_POSITION) { } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT) { } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.LOCAL_POSITION_NED) { } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG) { } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM) { } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_INT) { } byte[] datearray = BitConverter.GetBytes( (UInt64)((DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds * 1000)); Array.Reverse(datearray); outfilestream.Write(datearray, 0, datearray.Length); outfilestream.Write(packet.buffer, 0, packet.Length); } } }
public static void ProcessFile(string logfile) { if (File.Exists(logfile + ".jpg")) { return; } 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")) { Comms.CommsFile cf = new CommsFile(); cf.Open(logfile); using (CommsStream cs = new CommsStream(cf, cf.BytesToRead)) { MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse(true); while (cs.Position < cs.Length) { try { MAVLink.MAVLinkMessage packet = parse.ReadPacket(cs); if (packet == null || 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 = packet.sysid * 256 + 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); } } catch { } } } cf.Close(); } else if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log")) { using ( DFLogBuffer colbuf = new DFLogBuffer(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) ) { loc_list[0] = new List <PointLatLngAlt>(); foreach (var item in colbuf.GetEnumeratorType("GPS")) { if (item.msgtype.StartsWith("GPS")) { if (!colbuf.dflog.logformat.ContainsKey("GPS")) { continue; } var status = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Status")]); var lat = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Lat")]); var lon = double.Parse(item.items[colbuf.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); using (var map = GetMap(area)) using (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", SKEncodedImageFormat.Jpeg); File.SetLastWriteTime(logfile + ".jpg", new FileInfo(logfile).LastWriteTime); } } else { DoTextMap(logfile + ".jpg", "No gps data"); File.SetLastWriteTime(logfile + ".jpg", new FileInfo(logfile).LastWriteTime); } } catch (Exception ex) { if (ex.ToString().Contains("Mavlink 0.9")) { DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9"); } } }