public static string tlogToCSV(string filepath) { CurrentState.SpeedUnit = "m/s"; CurrentState.DistanceUnit = "m"; MAVLinkInterface proto = new MAVLinkInterface(); OpenFileDialog openFileDialog1 = new OpenFileDialog(); string LogFilePath; openFileDialog1.FileName = filepath; foreach (string logfile in openFileDialog1.FileNames) { using (MAVLinkInterface mine = new MAVLinkInterface()) { try { mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { log.Debug(ex.ToString()); } mine.logreadmode = true; mine.MAV.packets.Initialize(); // clear StreamWriter sw = new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".csv"); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); string text = ""; mine.DebugPacket(packet, ref text, true, ","); sw.Write(mine.lastlogread.ToString("yyyy-MM-ddTHH:mm:ss.fff") + "," + text); } sw.Close(); mine.logreadmode = false; mine.logplaybackfile.Close(); mine.logplaybackfile = null; LogFilePath = (Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + (Path.GetFileNameWithoutExtension(logfile) + ".csv")); return LogFilePath; } } return null; }
// Return List with all GPS Messages splitted in string arrays Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn) { Dictionary<long, VehicleLocation> vehiclePositionList = new Dictionary<long,VehicleLocation>(); // Telemetry Log if (fn.ToLower().EndsWith("tlog")) { MAVLinkInterface mine = new MAVLinkInterface(); mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read)); mine.logreadmode = true; mine.MAV.packets.Initialize(); // clear CurrentState cs = new CurrentState(); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); cs.datetime = mine.lastlogread; cs.UpdateCurrentSettings(null, true, mine); VehicleLocation location = new VehicleLocation(); location.Time = cs.datetime; location.Lat = cs.lat; location.Lon = cs.lng; location.RelAlt = cs.alt; location.AltAMSL = cs.altasl; location.Roll = cs.roll; location.Pitch = cs.pitch; location.Yaw = cs.yaw; vehiclePositionList[ToMilliseconds(location.Time)] = location; // 4 5 7 Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + " \r"); } mine.logplaybackfile.Close(); } // DataFlash Log else { StreamReader sr = new StreamReader(fn); // Will hold the last seen Attitude information in order to incorporate them into the GPS Info float currentYaw = 0f; float currentRoll = 0f; float currentPitch = 0f; while (!sr.EndOfStream) { string line = sr.ReadLine(); // Look for GPS Messages. However GPS Messages do not have Roll, Pitch and Yaw // So we have to look for one ATT message after having read a GPS one if (line.ToLower().StartsWith("gps")) { VehicleLocation location = new VehicleLocation(); string[] gpsLineValues = line.Split(new char[] { ',', ':' }); try { location.Time = GetTimeFromGps(int.Parse(getValueFromStringArray(gpsLineValues, gpsweekpos), CultureInfo.InvariantCulture), int.Parse(getValueFromStringArray(gpsLineValues, timepos), CultureInfo.InvariantCulture)); location.Lat = double.Parse(getValueFromStringArray(gpsLineValues, latpos), CultureInfo.InvariantCulture); location.Lon = double.Parse(getValueFromStringArray(gpsLineValues, lngpos), CultureInfo.InvariantCulture); location.RelAlt = double.Parse(getValueFromStringArray(gpsLineValues, altpos), CultureInfo.InvariantCulture); location.AltAMSL = double.Parse(getValueFromStringArray(gpsLineValues, altAMSLpos), CultureInfo.InvariantCulture); location.Roll = currentRoll; location.Pitch = currentPitch; location.Yaw = currentYaw; long millis = ToMilliseconds(location.Time); //System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis + " GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + " TimeMS = " + getValueFromStringArray(gpsLineValues, timepos)); if (!vehiclePositionList.ContainsKey(millis)) vehiclePositionList[millis] = location; } catch { Console.WriteLine("Bad GPS Line"); } } else if (line.ToLower().StartsWith("att")) { string[] attLineValues = line.Split(new char[] { ',', ':' }); currentRoll = float.Parse(getValueFromStringArray(attLineValues, rollATT), CultureInfo.InvariantCulture); currentPitch = float.Parse(getValueFromStringArray(attLineValues, pitchATT), CultureInfo.InvariantCulture); currentYaw = float.Parse(getValueFromStringArray(attLineValues, yawATT), CultureInfo.InvariantCulture); } } sr.Close(); } return vehiclePositionList; }
void dolog() { flightdata.Clear(); using (MAVLinkInterface mine = new MAVLinkInterface()) { try { mine.logplaybackfile = new BinaryReader(File.Open(txt_tlog.Text, 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 mine.readPacket(); startlogtime = mine.lastlogread; double oldlatlngsum = 0; int appui = 0; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); cs.datetime = mine.lastlogread; cs.UpdateCurrentSettings(null, true, mine); if (appui != DateTime.Now.Second) { // cant do entire app as mixes with flightdata timer this.Refresh(); appui = DateTime.Now.Second; } try { if (MainV2.speechEngine != null) MainV2.speechEngine.SpeakAsyncCancelAll(); } catch { } // ignore because of this Exception System.PlatformNotSupportedException: No voice installed on the system or none available with the current security setting. // if ((float)(cs.lat + cs.lng + cs.alt) != oldlatlngsum // && cs.lat != 0 && cs.lng != 0) DateTime nexttime = mine.lastlogread.AddMilliseconds(-(mine.lastlogread.Millisecond%100)); if (!flightdata.ContainsKey(nexttime)) { Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " + (float) (cs.lat + cs.lng + cs.alt) + "!=" + oldlatlngsum); CurrentState cs2 = (CurrentState) cs.Clone(); try { flightdata.Add(nexttime, cs2); } catch { } oldlatlngsum = (cs.lat + cs.lng + cs.alt); } } mine.logreadmode = false; mine.logplaybackfile.Close(); mine.logplaybackfile = null; } }
private void start_Terminal(bool px4) { setcomport(); try { if (MainV2.comPort != null && MainV2.comPort.BaseStream != null && MainV2.comPort.BaseStream.IsOpen) MainV2.comPort.BaseStream.Close(); if (comPort.IsOpen) { Console.WriteLine("Terminal Start - Close Port"); threadrun = false; // if (DialogResult.Cancel == CustomMessageBox.Show("The port is open\n Continue?", "Continue", MessageBoxButtons.YesNo)) { // return; } comPort.Close(); // allow things to cleanup System.Threading.Thread.Sleep(400); } comPort.ReadBufferSize = 1024 * 1024 * 4; comPort.PortName = MainV2.comPortName; // test moving baud rate line comPort.BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text); if (px4) { TXT_terminal.AppendText("Rebooting\n"); // keep it local MAVLinkInterface mine = new MAVLinkInterface(); mine.BaseStream.PortName = MainV2.comPortName; mine.BaseStream.BaudRate = comPort.BaudRate; mine.giveComport = true; mine.BaseStream.Open(); // check if we are a mavlink stream byte[] buffer = mine.readPacket(); if (buffer.Length > 0) { log.Info("got packet - sending reboot via mavlink"); TXT_terminal.AppendText("Via Mavlink\n"); mine.doReboot(false); try { mine.BaseStream.Close(); } catch { } } else { log.Info("no packet - sending reboot via console"); TXT_terminal.AppendText("Via Console\n"); MainV2.comPort.BaseStream.Write("exit\rreboot\r"); try { MainV2.comPort.BaseStream.Close(); } catch { } } TXT_terminal.AppendText("Waiting for reboot\n"); // wait 7 seconds for px4 reboot log.Info("waiting for reboot"); DateTime deadline = DateTime.Now.AddSeconds(8); while (DateTime.Now < deadline) { System.Threading.Thread.Sleep(500); Application.DoEvents(); } int a = 0; while (a < 5) { try { if (!comPort.IsOpen) comPort.Open(); } catch { } System.Threading.Thread.Sleep(200); a++; } } else { log.Info("About to open " + comPort.PortName); comPort.Open(); comPort.toggleDTR(); } try { comPort.DiscardInBuffer(); } catch { } Console.WriteLine("Terminal_Load run " + threadrun + " " + comPort.IsOpen); BUT_disconnect.Enabled = true; System.Threading.Thread t11 = new System.Threading.Thread(delegate() { threadrun = true; Console.WriteLine("Terminal thread start run run " + threadrun + " " + comPort.IsOpen); // 10 sec waitandsleep(10000); Console.WriteLine("Terminal thread 1 run " + threadrun + " " + comPort.IsOpen); // 100 ms readandsleep(100); Console.WriteLine("Terminal thread 2 run " + threadrun + " " + comPort.IsOpen); try { if (!inlogview && comPort.IsOpen) comPort.Write("\n\n\n"); // 1 secs if (!inlogview && comPort.IsOpen) readandsleep(1000); if (!inlogview && comPort.IsOpen) comPort.Write("\r\r\r?\r"); } catch (Exception ex) { Console.WriteLine("Terminal thread 3 " + ex.ToString()); ChangeConnectStatus(false); threadrun = false; return; } Console.WriteLine("Terminal thread 3 run " + threadrun + " " + comPort.IsOpen); while (threadrun) { try { System.Threading.Thread.Sleep(10); if (!threadrun) break; if (this.Disposing) break; if (inlogview) continue; if (!comPort.IsOpen) { Console.WriteLine("Comport Closed"); ChangeConnectStatus(false); break; } if (comPort.BytesToRead > 0) { comPort_DataReceived((object)null, (SerialDataReceivedEventArgs)null); } } catch (Exception ex) { Console.WriteLine("Terminal thread 4 " + ex.ToString()); } } threadrun = false; try { comPort.DtrEnable = false; } catch { } try { Console.WriteLine("term thread close run " + threadrun + " " + comPort.IsOpen); ChangeConnectStatus(false); comPort.Close(); } catch { } Console.WriteLine("Comport thread close run " + threadrun); }); t11.IsBackground = true; t11.Name = "Terminal serial thread"; t11.Start(); // doesnt seem to work on mac //comPort.DataReceived += new SerialDataReceivedEventHandler(comPort_DataReceived); if (this.IsDisposed || this.Disposing) return; TXT_terminal.AppendText("Opened com port\r\n"); inputStartPos = TXT_terminal.SelectionStart; } catch (Exception ex) { log.Error(ex); TXT_terminal.AppendText("Cant open serial port\r\n"); return; } TXT_terminal.Focus(); }
/// <summary> /// Processes a tlog to get the offsets - creates dxf of data /// </summary> /// <param name="fn">Filename</param> /// <returns>Offsets</returns> public static double[] getOffsets(string fn, int throttleThreshold = 0) { // based off tridge's work string logfile = fn; // old method float minx = 0; float maxx = 0; float miny = 0; float maxy = 0; float minz = 0; float maxz = 0; // this is for a dxf Polyline3dVertex vertex; List<Polyline3dVertex> vertexes = new List<Polyline3dVertex>(); // data storage Tuple<float, float, float> offset = new Tuple<float, float, float>(0, 0, 0); List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>(); Hashtable filter = new Hashtable(); // track data to use bool useData = false; if (throttleThreshold <= 0) useData = true; log.Info("Start log: " + DateTime.Now); using (MAVLinkInterface mine = new MAVLinkInterface()) { try { mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return new double[] {0}; } mine.logreadmode = true; mine.MAV.packets.Initialize(); // clear // gather data while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packetraw = mine.readPacket(); var packet = mine.DebugPacket(packetraw, false); // this is for packets we dont know about if (packet == null) continue; if (packet.GetType() == typeof (MAVLink.mavlink_vfr_hud_t)) { if (((MAVLink.mavlink_vfr_hud_t) packet).throttle >= throttleThreshold) { useData = true; } else { useData = false; } } if (packet.GetType() == typeof (MAVLink.mavlink_sensor_offsets_t)) { offset = new Tuple<float, float, float>( ((MAVLink.mavlink_sensor_offsets_t) packet).mag_ofs_x, ((MAVLink.mavlink_sensor_offsets_t) packet).mag_ofs_y, ((MAVLink.mavlink_sensor_offsets_t) packet).mag_ofs_z); } else if (packet.GetType() == typeof (MAVLink.mavlink_raw_imu_t) && useData) { int div = 20; // fox dxf vertex = new Polyline3dVertex(new Vector3f( ((MAVLink.mavlink_raw_imu_t) packet).xmag - offset.Item1, ((MAVLink.mavlink_raw_imu_t) packet).ymag - offset.Item2, ((MAVLink.mavlink_raw_imu_t) packet).zmag - offset.Item3) ); vertexes.Add(vertex); // for old method setMinorMax(((MAVLink.mavlink_raw_imu_t) packet).xmag - offset.Item1, ref minx, ref maxx); setMinorMax(((MAVLink.mavlink_raw_imu_t) packet).ymag - offset.Item2, ref miny, ref maxy); setMinorMax(((MAVLink.mavlink_raw_imu_t) packet).zmag - offset.Item3, ref minz, ref maxz); // for new lease sq string item = (int) (((MAVLink.mavlink_raw_imu_t) packet).xmag/div) + "," + (int) (((MAVLink.mavlink_raw_imu_t) packet).ymag/div) + "," + (int) (((MAVLink.mavlink_raw_imu_t) packet).zmag/div); if (filter.ContainsKey(item)) { filter[item] = (int) filter[item] + 1; if ((int) filter[item] > 3) continue; } else { filter[item] = 1; } data.Add(new Tuple<float, float, float>( ((MAVLink.mavlink_raw_imu_t) packet).xmag - offset.Item1, ((MAVLink.mavlink_raw_imu_t) packet).ymag - offset.Item2, ((MAVLink.mavlink_raw_imu_t) packet).zmag - offset.Item3)); } } log.Info("Log Processed " + DateTime.Now); Console.WriteLine("Extracted " + data.Count + " data points"); Console.WriteLine("Current offset: " + offset); mine.logreadmode = false; mine.logplaybackfile.Close(); mine.logplaybackfile = null; } if (data.Count < 10) { CustomMessageBox.Show("Log does not contain enough data"); throw new Exception("Not Enough Data"); } data.Sort( delegate(Tuple<float, float, float> d1, Tuple<float, float, float> d2) { // get distance from 0,0,0 double ans1 = Math.Sqrt(d1.Item1*d1.Item1 + d1.Item2*d1.Item2 + d1.Item3*d1.Item3); double ans2 = Math.Sqrt(d2.Item1*d2.Item1 + d2.Item2*d2.Item2 + d2.Item3*d2.Item3); if (ans1 > ans2) return 1; if (ans1 < ans2) return -1; return 0; } ); data.RemoveRange(data.Count - (data.Count/16), data.Count/16); System.Console.WriteLine("Old Method {0} {1} {2}", -(maxx + minx)/2, -(maxy + miny)/2, -(maxz + minz)/2); double[] x = LeastSq(data); log.Info("Least Sq Done " + DateTime.Now); doDXF(vertexes, x); Array.Resize<double>(ref x, 3); return x; }
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.speechenabled = false; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { MAVLink.MAVLinkMessage packet = mine.readPacket(); object data = packet.data; 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; } }
List<string[]> readLog(string fn) { if (logcache.Count > 0) return logcache; List<string[]> list = new List<string[]>(); if (fn.ToLower().EndsWith("tlog")) { MAVLinkInterface mine = new MAVLinkInterface(); mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read)); mine.logreadmode = true; mine.MAV.packets.Initialize(); // clear CurrentState cs = new CurrentState(); string[] oldvalues = {""}; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); cs.datetime = mine.lastlogread; cs.UpdateCurrentSettings(null, true, mine); // old // line "GPS: 82686250, 1, 8, -34.1406480, 118.5441900, 0.0000, 309.1900, 315.9500, 0.0000, 279.1200" string //Status,Time,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs //GPS, 3, 122732, 10, 0.00, -35.3628880, 149.1621961, 808.90, 810.30, 23.30, 94.04 string[] vals = new string[] { "GPS", "3", (cs.datetime.ToUniversalTime() - new DateTime(cs.datetime.Year,cs.datetime.Month,cs.datetime.Day,0,0,0,DateTimeKind.Utc)).TotalMilliseconds.ToString(), cs.satcount.ToString(),cs.gpshdop.ToString(),cs.lat.ToString(),cs.lng.ToString(),cs.altasl.ToString(),cs.altasl.ToString(),cs.groundspeed.ToString(),cs.yaw.ToString()}; if (oldvalues.Length > 2 && oldvalues[latpos] == vals[latpos] && oldvalues[lngpos] == vals[lngpos] && oldvalues[altpos] == vals[altpos]) continue; oldvalues = vals; list.Add(vals); // 4 5 7 Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + " \r"); } mine.logplaybackfile.Close(); logcache = list; return list; } StreamReader sr = new StreamReader(fn); string lasttime = "0"; while (!sr.EndOfStream) { string line = sr.ReadLine(); if (line.ToLower().StartsWith("gps")) { if (!sr.EndOfStream) { string line2 = sr.ReadLine(); if (line2.ToLower().StartsWith("att")) { line = string.Concat(line, ",", line2); } } string[] vals = line.Split(new char[] {',',':'}); if (lasttime == vals[timepos]) continue; lasttime = vals[timepos]; list.Add(vals); } } sr.Close(); logcache = list; return list; }
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) { MAVLink.MAVLinkMessage packetbytes = mavint.readPacket(); if (packetbytes.Length < 5) continue; object packet = packetbytes.data; 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; }
private void but_droneapi_Click(object sender, EventArgs e) { string droneshareusername = MainV2.getConfig("droneshareusername"); InputBox.Show("Username", "Username", ref droneshareusername); MainV2.config["droneshareusername"] = droneshareusername; string dronesharepassword = MainV2.getConfig("dronesharepassword"); if (dronesharepassword != "") { try { // fail on bad entry var crypto = new Crypto(); dronesharepassword = crypto.DecryptString(dronesharepassword); } catch { } } InputBox.Show("Password", "Password", ref dronesharepassword, true); var crypto2 = new Crypto(); string encryptedpw = crypto2.EncryptString(dronesharepassword); MainV2.config["dronesharepassword"] = encryptedpw; DroneProto dp = new DroneProto(); if (dp.connect()) { if (dp.loginUser(droneshareusername, dronesharepassword)) { MAVLinkInterface mine = new MAVLinkInterface(); var comfile = new CommsFile(); mine.BaseStream = comfile; mine.BaseStream.PortName = @"C:\Users\hog\Documents\apm logs\iris 6-4-14\2014-04-06 09-07-32.tlog"; mine.BaseStream.Open(); comfile.bps = 4000; mine.getHeartBeat(); dp.setVechileId(mine.MAV.Guid, 0, mine.MAV.sysid); dp.startMission(); while (true) { byte[] packet = mine.readPacket(); dp.SendMavlink(packet, 0); } // dp.close(); // mine.Close(); } } }
// Return List with all GPS Messages splitted in string arrays Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn) { Dictionary<long, VehicleLocation> vehiclePositionList = new Dictionary<long,VehicleLocation>(); // Telemetry Log if (fn.ToLower().EndsWith("tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) { mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read)); mine.logreadmode = true; mine.MAV.packets.Initialize(); // clear CurrentState cs = new CurrentState(); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); cs.datetime = mine.lastlogread; cs.UpdateCurrentSettings(null, true, mine); VehicleLocation location = new VehicleLocation(); location.Time = cs.datetime; location.Lat = cs.lat; location.Lon = cs.lng; location.RelAlt = cs.alt; location.AltAMSL = cs.altasl; location.Roll = cs.roll; location.Pitch = cs.pitch; location.Yaw = cs.yaw; vehiclePositionList[ToMilliseconds(location.Time)] = location; // 4 5 7 Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + " \r"); } mine.logplaybackfile.Close(); } } // DataFlash Log else { // convert bin to log if (fn.ToLower().EndsWith("bin")) { string tempfile = Path.GetTempFileName(); Log.BinaryLog.ConvertBin(fn, tempfile); fn = tempfile; } StreamReader sr = new StreamReader(fn); // Will hold the last seen Attitude information in order to incorporate them into the GPS Info float currentYaw = 0f; float currentRoll = 0f; float currentPitch = 0f; int a = 0; while (!sr.EndOfStream) { a++; string line = sr.ReadLine(); var item = DFLog.GetDFItemFromLine(line, a); if (item.msgtype == "GPS") { if (!DFLog.logformat.ContainsKey("GPS")) continue; int latindex = DFLog.FindMessageOffset("GPS", "Lat"); int lngindex = DFLog.FindMessageOffset("GPS", "Lng"); int altindex = DFLog.FindMessageOffset("GPS", "Alt"); int raltindex = DFLog.FindMessageOffset("GPS", "RAlt"); if (raltindex == -1) raltindex = DFLog.FindMessageOffset("GPS", "RelAlt"); VehicleLocation location = new VehicleLocation(); try { location.Time = DFLog.GetTimeGPS(line); location.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture); location.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture); location.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture); location.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture); location.Roll = currentRoll; location.Pitch = currentPitch; location.Yaw = currentYaw; long millis = ToMilliseconds(location.Time); //System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis + " GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + " TimeMS = " + getValueFromStringArray(gpsLineValues, timepos)); if (!vehiclePositionList.ContainsKey(millis) && (location.Time != DateTime.MinValue)) vehiclePositionList[millis] = location; } catch { Console.WriteLine("Bad GPS Line"); } } else if (item.msgtype == "ATT") { int Rindex = DFLog.FindMessageOffset("ATT", "Roll"); int Pindex = DFLog.FindMessageOffset("ATT", "Pitch"); int Yindex = DFLog.FindMessageOffset("ATT", "Yaw"); currentRoll = float.Parse(item.items[Rindex], CultureInfo.InvariantCulture); currentPitch = float.Parse(item.items[Pindex], CultureInfo.InvariantCulture); currentYaw = float.Parse(item.items[Yindex], CultureInfo.InvariantCulture); } // Look for GPS Messages. However GPS Messages do not have Roll, Pitch and Yaw // So we have to look for one ATT message after having read a GPS one //if (line.ToLower().StartsWith("gps")) //{ // VehicleLocation location = new VehicleLocation(); // string[] gpsLineValues = line.Split(new char[] { ',', ':' }); // try // { // location.Time = GetTimeFromGps(int.Parse(getValueFromStringArray(gpsLineValues, gpsweekpos), CultureInfo.InvariantCulture), int.Parse(getValueFromStringArray(gpsLineValues, timepos), CultureInfo.InvariantCulture)); // location.Lat = double.Parse(getValueFromStringArray(gpsLineValues, latpos), CultureInfo.InvariantCulture); // location.Lon = double.Parse(getValueFromStringArray(gpsLineValues, lngpos), CultureInfo.InvariantCulture); // location.RelAlt = double.Parse(getValueFromStringArray(gpsLineValues, altpos), CultureInfo.InvariantCulture); // location.AltAMSL = double.Parse(getValueFromStringArray(gpsLineValues, altAMSLpos), CultureInfo.InvariantCulture); // location.Roll = currentRoll; // location.Pitch = currentPitch; // location.Yaw = currentYaw; // long millis = ToMilliseconds(location.Time); // //System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis + " GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + " TimeMS = " + getValueFromStringArray(gpsLineValues, timepos)); // if (!vehiclePositionList.ContainsKey(millis)) // vehiclePositionList[millis] = location; // } // catch { Console.WriteLine("Bad GPS Line"); } //} //else if (line.ToLower().StartsWith("att")) //{ // string[] attLineValues = line.Split(new char[] { ',', ':' }); // currentRoll = float.Parse(getValueFromStringArray(attLineValues, rollATT), CultureInfo.InvariantCulture); // currentPitch = float.Parse(getValueFromStringArray(attLineValues, pitchATT), CultureInfo.InvariantCulture); // currentYaw = float.Parse(getValueFromStringArray(attLineValues, yawATT), CultureInfo.InvariantCulture); //} } sr.Close(); } return vehiclePositionList; }
private void but_gpsinj_Click(object sender, EventArgs e) { OpenFileDialog ofd = new OpenFileDialog(); ofd.Filter = "tlog|*.tlog"; ofd.ShowDialog(); SaveFileDialog sfd = new SaveFileDialog(); sfd.FileName = "output.dat"; sfd.ShowDialog(); if (ofd.CheckFileExists) { using (var st = sfd.OpenFile()) { using (MAVLinkInterface mine = new MAVLinkInterface(ofd.OpenFile())) { mine.logreadmode = true; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { MAVLink.MAVLinkMessage packet = mine.readPacket(); if (packet.msgid == (uint) MAVLink.MAVLINK_MSG_ID.GPS_INJECT_DATA) { var item = packet.ToStructure<MAVLink.mavlink_gps_inject_data_t>(); st.Write(item.data, 0, item.len); } } } } } }
private void start_Terminal(bool px4) { try { if (MainV2.comPort != null && MainV2.comPort.BaseStream != null && MainV2.comPort.BaseStream.IsOpen) MainV2.comPort.BaseStream.Close(); setcomport(); if (px4) { TXT_terminal.AppendText("Rebooting " + MainV2.comPortName + " at " + comPort.BaudRate + "\n"); // keep it local using (MAVLinkInterface mine = new MAVLinkInterface()) { mine.BaseStream.PortName = MainV2.comPortName; mine.BaseStream.BaudRate = comPort.BaudRate; mine.giveComport = true; mine.BaseStream.Open(); // check if we are a mavlink stream byte[] buffer = mine.readPacket(); if (buffer.Length > 0) { log.Info("got packet - sending reboot via mavlink"); TXT_terminal.AppendText("Via Mavlink\n"); mine.doReboot(false); try { mine.BaseStream.Close(); } catch { } } else { log.Info("no packet - sending reboot via console"); TXT_terminal.AppendText("Via Console\n"); try { mine.BaseStream.Write("reboot\r"); mine.BaseStream.Write("exit\rreboot\r"); } catch { } try { mine.BaseStream.Close(); } catch { } } } TXT_terminal.AppendText("Waiting for reboot\n"); // wait 7 seconds for px4 reboot log.Info("waiting for reboot"); DateTime deadline = DateTime.Now.AddSeconds(9); while (DateTime.Now < deadline) { System.Threading.Thread.Sleep(500); Application.DoEvents(); } int a = 0; while (a < 5) { try { if (!comPort.IsOpen) comPort.Open(); } catch { } System.Threading.Thread.Sleep(200); a++; } } else { log.Info("About to open " + comPort.PortName); comPort.Open(); log.Info("Toggle dtr"); comPort.toggleDTR(); } try { comPort.DiscardInBuffer(); } catch { } Console.WriteLine("Terminal_Load run " + threadrun + " " + comPort.IsOpen); t11 = new System.Threading.Thread(delegate() { threaderror = false; threadrun = true; Console.WriteLine("Terminal thread startup 1 " + threadrun + " " + comPort.IsOpen); try { comPort.Write("\r"); // 10 sec waitandsleep(10000); Console.WriteLine("Terminal thread startup 2 " + threadrun + " " + comPort.IsOpen); // 100 ms readandsleep(100); } catch (Exception ex) { Console.WriteLine("Terminal thread error 3 " + ex.ToString()); threaderror = true; } Console.WriteLine("Terminal thread startup 3 " + threadrun + " " + comPort.IsOpen); try { comPort.Write("\n\n\n"); // 1 secs readandsleep(1000); comPort.Write("\r\r\r?\r"); } catch (Exception ex) { if (!threaderror) Console.WriteLine("Terminal thread error 4 " + ex.ToString()); threaderror = true; } Console.WriteLine("Terminal thread startup 4 " + threadrun + " " + comPort.IsOpen); if (!threaderror) setButtonState(true); while (threadrun && !threaderror) { try { System.Threading.Thread.Sleep(10); if (inlogview) continue; if (comPort.BytesToRead > 0) { comPort_DataReceived((object)null, (SerialDataReceivedEventArgs)null); } } catch (Exception ex) { Console.WriteLine("Terminal thread error 5 " + ex.ToString()); threaderror = true; } } try { //comPort.Write("\rreboot\r"); comPort.DtrEnable = false; } catch { } try { Console.WriteLine("Terminal thread close port"); comPort.Close(); } catch { } setButtonState(false); while (threadrun) { //stay in thread if threaderror System.Threading.Thread.Sleep(10); } log.Info("Terminal thread exit"); }); t11.IsBackground = true; t11.Name = "Terminal serial thread"; t11.Start(); // doesnt seem to work on mac //comPort.DataReceived += new SerialDataReceivedEventHandler(comPort_DataReceived); BUT_ConnectAPM.Enabled = false; BUT_disconnect.Enabled = true; TXT_terminal.AppendText("Opened com port\r\n"); inputStartPos = TXT_terminal.SelectionStart; } catch (Exception ex) { log.Error(ex); TXT_terminal.AppendText("Cant open com port\r\n"); return; } TXT_terminal.Focus(); }