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);
}
public static void tlog(string logfile)
{
List <MLArray> mlList = new List <MLArray>();
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
Hashtable datappl = new Hashtable();
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;
}
}
