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