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;
}
}
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;
}
