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",