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