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 Create(string filein, string fileout, List <string> fmtList = null) { using (StreamReader tr = new StreamReader(filein)) using (DFLogBuffer logdata = new DFLogBuffer(tr.BaseStream)) { List <string> colList = new List <string>(); Dictionary <string, int> colStart = new Dictionary <string, int>(); colStart.Add("GLOBAL", colList.Count); colList.Add("GLOBAL_TimeMS"); foreach (var logformatValue in logdata.dflog.logformat.Values) { if (fmtList != null && !fmtList.Contains(logformatValue.Name)) { continue; } colStart.Add(logformatValue.Name, colList.Count); foreach (var field in logformatValue.FieldNames) { colList.Add(logformatValue.Name + "_" + field); } } using (StreamWriter sr = new StreamWriter(fileout)) { // header foreach (var item in colList) { sr.Write(item + ","); } sr.WriteLine(); // lines foreach (var dfitem in logdata.GetEnumeratorType(colStart.Keys.ToArray())) { if (dfitem.msgtype == "FMT") { continue; } var idx = 0; StringBuilder sb = new StringBuilder(); sb.Append(dfitem.timems); idx++; sb.Append(','); var start = colStart[dfitem.msgtype]; while (idx < start) { idx++; sb.Append(','); } foreach (var item in dfitem.items.Skip(1)) { sb.Append(item?.Trim()); idx++; sb.Append(','); } sr.WriteLine(sb); } } } }
public static void getdata(string file) { DFLog.DFItem? IMU = null; DFLog.DFItem? ATT = null; List <Vector3> gaccel = new List <Vector3>(); List <Vector3> vel = new List <Vector3>(); List <DateTime> timestamps = new List <DateTime>(); List <int> accel_indexes = new List <int>(); var dtsum = 0.0; var dtcount = 0; var cb = new DFLogBuffer(File.OpenRead(file)); foreach (var item in cb.GetEnumeratorType(new string[] { "GPS", "GPS2", "IMU", "ATT" })) { switch (item.msgtype) { case "GPS": case "GPS2": if ((Byte)item.GetRaw("Status") >= 3) { var v = new Vector3( (Single)item.GetRaw("Spd") * Math.Cos(radians((Single)item.GetRaw("GCrs"))), (Single)item.GetRaw("Spd") * Math.Sin(radians((Single)item.GetRaw("GCrs"))), (Single)item.GetRaw("VZ")); vel.Add(v); timestamps.Add(item.time); accel_indexes.Add(Math.Max(gaccel.Count - 1, 0)); } break; case "IMU": if (ATT.HasValue) { gaccel.Add(earth_accel_df(item, ATT.Value)); if (IMU.HasValue) { var dt = item.time - IMU.Value.time; dtsum += dt.TotalSeconds; dtcount += 1; } IMU = item; } break; case "ATT": ATT = item; break; } } var imu_dt = dtsum / dtcount; Console.WriteLine("{0} samples at dt {1}", vel.Count, imu_dt); var besti = -1; var besterr = 0.0; List <double> delays = new List <double>(); List <double> errors = new List <double>(); foreach (var i in Enumerable.Range(0, 100)) { var err = velocity_error(timestamps, vel, gaccel, accel_indexes, imu_dt, i); if (err == 0) { break; } errors.Add(err); delays.Add(i * imu_dt); if (besti == -1 || err < besterr) { besti = i; besterr = err; } } Console.WriteLine("Best {0} ({1}s) {2}", besti, besti * imu_dt, besterr); }
public DFLog(DFLogBuffer dfLogBuffer) { _dfLogBuffer = dfLogBuffer; }
public static Image <Rgba32> GenerateImage(DFLogBuffer cb, out double[] freqtout, out List <(double timeus, double[] value)> allfftdata, string type = "ACC1", string field = "AccX",