public static unsafe void ExtractLogFile(string filename)
{
// open txt file
StreamWriter sw = File.CreateText("matlab.txt");
// create header
sw.Write("Loop ");
sw.Write("Voltage FuelLevel CurrentA TotalCharge_mAh ");
sw.Write("MagX MagY MagZ ");
sw.Write("AccX AccY AccZ ");
sw.Write("Roll Pitch Yaw ");
sw.Write("dRoll dPitch dYaw ");
sw.Write("Pressure Temperature Altitude Vertspeed ");
sw.Write("PWM1 PWM2 PWM3 PWM4 ");
sw.Write("NumSV VelN VelE VelD HeightMSL HorizontalAccuracy ");
for (int i = 0; i != 10; i++)
{
sw.Write("D{0} ", i);
}
sw.WriteLine("Mode HopeRXRSSI HopeRXFrameCnt HopeRSSI ");
sw.WriteLine("");
SCommEthData data = new SCommEthData();
FileStream logStream = File.OpenRead(filename);
int size = Marshal.SizeOf(data);
byte[] dataArray = new byte[size];
while (logStream.Read(dataArray, 0, size) > 0)
{
data = (SCommEthData)Comm.FromBytes(dataArray, new SCommEthData());
sw.Write("{0} ", data.LoopCounter);
sw.Write("{0} {1} {2} {3} ", data.BatteryVoltage, data.FuelLevel, data.BatteryCurrentA, data.BatteryTotalCharge_mAh);
sw.Write("{0} {1} {2} ", data.MagX, data.MagY, data.MagZ);
sw.Write("{0} {1} {2} ", data.AccX, data.AccY, data.AccZ);
sw.Write("{0} {1} {2} ", data.Roll, data.Pitch, data.Yaw);
sw.Write("{0} {1} {2} ", data.dRoll, data.dPitch, data.dYaw);
sw.Write("{0} {1} {2} {3} ", data.Pressure, data.Temperature, data.Altitude, data.Vertspeed);
sw.Write("{0} {1} {2} {3} ", data.MotorThrusts[0], data.MotorThrusts[1], data.MotorThrusts[2], data.MotorThrusts[3]);
sw.Write("{0} {1} {2} {3} {4} {5} ", data.NumSV, data.VelN / 1000.0, data.VelE / 1000.0, data.VelD / 1000.0, data.HeightMSL / 1000.0, data.HorizontalAccuracy / 1000.0);
for (int i = 0; i != 10; i++)
{
sw.Write("{0} ", data.TuningData[i]);
}
sw.Write("{0} {1} {2} {3} ", data.ActualMode, data.HopeRXRSSI, data.HopeRXFrameCount, data.HopeRSSI);
sw.WriteLine("");
}
sw.Close();
}
public unsafe void DisplaySystemData(int ReceivedPacketsCounter, SCommEthData MainSystemData)
{
if (radioButtonWifi.Checked || radioButtonWifiHopeRF.Checked)
{
// GPS
textBoxLocTime.Text = MainSystemData.GPSTime.ToString();
textBoxLocSatNr.Text = MainSystemData.NumSV.ToString();
textBoxNavLocSatNr.Text = MainSystemData.NumSV.ToString();
textBoxLocLat.Text = (MainSystemData.Latitude * 1e-7).ToString("0.000000");
textBoxLocLon.Text = (MainSystemData.Longitude * 1e-7).ToString("0.000000");
textBoxHomeLat.Text = (MainSystemData.HomeLatitude * 1e-7).ToString("0.000000");
textBoxHomeLon.Text = (MainSystemData.HomeLongitude * 1e-7).ToString("0.000000");
textBoxLocVelocityN.Text = (MainSystemData.VelN / 1000.0).ToString("0.00 m/s");
textBoxLocVelocityE.Text = (MainSystemData.VelE / 1000.0).ToString("0.00 m/s");
textBoxLocPressure.Text = (MainSystemData.Pressure / 100).ToString("0.00 hPa");
textBoxLocTemperature.Text = (MainSystemData.Temperature).ToString("0.0 °C");
textBoxLocAlt.Text = (MainSystemData.Altitude).ToString("0.0 m");
textBoxLocVertSpeed.Text = (MainSystemData.Vertspeed).ToString("0.0 m/s");
textBoxLocAssist.Text = MainSystemData.AssistNextChunkToSend.ToString();
string cnos1 = "";
string cnos2 = "";
for (int i = 0; i != 16; i++)
{
cnos1 += string.Format("{0}", MainSystemData.SatCNOs[i]) + Environment.NewLine;
cnos2 += string.Format("{0}", MainSystemData.SatCNOs[i + 16]) + Environment.NewLine;
}
textBoxSatCNOs1.Text = cnos1;
textBoxSatCNOs2.Text = cnos2;
// Attitude
textBoxRoll.Text = MainSystemData.Roll.ToString("0.0 °");
textBoxPitch.Text = MainSystemData.Pitch.ToString("0.0 °");
textBoxYaw.Text = MainSystemData.Yaw.ToString("0.0 °");
textBoxAccX.Text = MainSystemData.AccX.ToString("0.0 m/s2");
textBoxAccY.Text = MainSystemData.AccY.ToString("0.0 m/s2");
textBoxAccZ.Text = MainSystemData.AccZ.ToString("0.0 m/s2");
textBoxMagX.Text = MainSystemData.MagX.ToString("0.0 uT");
textBoxMagY.Text = MainSystemData.MagY.ToString("0.0 uT");
textBoxMagZ.Text = MainSystemData.MagZ.ToString("0.0 uT");
textBoxdRoll.Text = MainSystemData.dRoll.ToString("0.0 °/s");
textBoxdPitch.Text = MainSystemData.dPitch.ToString("0.0 °/s");
textBoxdYaw.Text = MainSystemData.dYaw.ToString("0.0 °/s");
textBoxdRollFiltered.Text = FilteredRoll.ToString("0.000 °/s");
textBoxdPitchFiltered.Text = FilteredPitch.ToString("0.000 °/s");
textBoxdYawFiltered.Text = FilteredYaw.ToString("0.000 °/s");
// System
textBoxSystemEthFrames.Text = ReceivedPacketsCounter.ToString();
textBoxSystemEthSent.Text = MainSystemData.EthSentCount.ToString();
textBoxSystemEthReceived.Text = MainSystemData.EthReceivedCount.ToString();
textBoxSystemLoop.Text = MainSystemData.LoopCounter.ToString();
textBoxSystemLoopTime.Text = MainSystemData.PerfLoopTimeMS.ToString("0.000 ms");
textBoxSystemCPUTime.Text = MainSystemData.PerfCpuTimeMS.ToString("0.000 ms");
textBoxSystemCPUTimeMax.Text = MainSystemData.PerfCpuTimeMSMAX.ToString("0.000 ms");
textBoxSystemBattery.Text = MainSystemData.BatteryVoltage.ToString("0.00 V");
textBoxSystemBatteryPercent.Text = MainSystemData.FuelLevel.ToString("0.0 ") + "%";
textBoxNavigationBatteryPercent.Text = MainSystemData.FuelLevel.ToString("0.0 ") + "%";
textBoxSystemMode.Text = MainSystemData.ActualMode.ToString();
textBoxNavigationMode.Text = MainSystemData.ActualMode.ToString();
textBoxSystemT1.Text = MainSystemData.MotorThrusts[0].ToString();
textBoxSystemT2.Text = MainSystemData.MotorThrusts[1].ToString();
textBoxSystemT3.Text = MainSystemData.MotorThrusts[2].ToString();
textBoxSystemT4.Text = MainSystemData.MotorThrusts[3].ToString();
textBoxSystemBatteryCurrentA.Text = MainSystemData.BatteryCurrentA.ToString("0.0 A");
textBoxSystemBatteryTotalCharge_mAH.Text = MainSystemData.BatteryTotalCharge_mAh.ToString("0 mAh");
// HopeRF
textBoxHopeRXCount.Text = MainSystemData.HopeRXFrameCount.ToString();
textBoxHopeRSSI.Text = MainSystemData.HopeRSSI.ToString();
textBoxHopeRXRSSI.Text = MainSystemData.HopeRXRSSI.ToString();
textBoxWaypointsCnt.Text = MainSystemData.WaypointCnt.ToString();
textBoxWaypointsDownloadCnt.Text = MainSystemData.WaypointDownloadCounter.ToString();
// Download map
textBoxMapDownloadLat.Text = (MainSystemData.Latitude * 1e-7).ToString("0.000000");
textBoxMapDownloadLon.Text = (MainSystemData.Longitude * 1e-7).ToString("0.000000");
// Launch
string[] launchStatus = { "Idle", "Armed", "Firing" };
textBoxWpnStatus1.Text = launchStatus[MainSystemData.LaunchStatus1];
textBoxWpnStatus2.Text = launchStatus[MainSystemData.LaunchStatus2];
// Update Home
navigation.SetHome(MainSystemData.HomeLongitude * 1e-7, MainSystemData.HomeLatitude * 1e-7);
// Update current position
navigation.UpdateCurrentPosition(MainSystemData.Altitude, MainSystemData.Longitude * 1e-7, MainSystemData.Latitude * 1e-7, MainSystemData.ActualMode);
}
}
///////////////////////////////
// DATA PARSER
///////////////////////////////
public unsafe void Update()
{
///////////////////////////////
// Process Serial Data
///////////////////////////////
commMgr.Update(10);
///////////////////////////////
// Process Ethernet Data
///////////////////////////////
if (udp == null)
{
return;
}
// Get data from UDP
while (udp.Available > 0)
{
// get data
IPEndPoint endP = new IPEndPoint(IPAddress.Any, 0); // any port of the sender
byte[] bytes = udp.Receive(ref endP);
// process data
ReceivedPacketsCounter++;
if (PacketIsValid(bytes))
{
// remove header
byte[] withoutHeader = bytes.Skip(3).ToArray();
switch (bytes[2])
{
case 0x20:
// data
SCommEthData commData = (SCommEthData)Comm.FromBytes(withoutHeader, new SCommEthData());
MainSystemData = commData;
// filter data
formMain.FilteredRoll = 0.9998 * formMain.FilteredRoll + 0.0002 * MainSystemData.dRoll;
formMain.FilteredPitch = 0.9998 * formMain.FilteredPitch + 0.0002 * MainSystemData.dPitch;
formMain.FilteredYaw = 0.9998 * formMain.FilteredYaw + 0.0002 * MainSystemData.dYaw;
for (int i = 0; i != 10; i++)
{
formMain.FilteredTuningData[i] = 0.999 * formMain.FilteredTuningData[i] + 0.001 * Math.Abs(commData.TuningData[i] - formMain.FilteredTuningLastData[i]);
formMain.FilteredTuningLastData[i] = commData.TuningData[i];
}
logStream.Write(withoutHeader, 0, withoutHeader.Length);
break;
case 0x62: // get params data
SParameters p = (SParameters)Comm.FromBytes(withoutHeader, new SParameters());
// Display
formMain.DisplayParams(p);
break;
}
}
}
// send PING
if (++PingCounter > 20)
{
PingCounter = 0;
byte[] lp = BitConverter.GetBytes(LocalPort); // send port
SendData(0x10, lp);
}
// GUI update
formMain.DisplaySystemData(ReceivedPacketsCounter, MainSystemData);
// Update assist now
byte[] toSend = assistNow.Update(MainSystemData.AssistNextChunkToSend);
if (toSend != null)
{
SendData(0x30, toSend);
}
}