///////////////////////////////
// Serial Parser + Stuff
///////////////////////////////
public void ProcessMessage(byte type, byte[] data, byte len)
{
// data
if (type == 0x20)
{
SCommHopeRFDataA2Avion commDataHopeRF = (SCommHopeRFDataA2Avion)Comm.FromBytes(data, new SCommHopeRFDataA2Avion());
RelayedData = commDataHopeRF;
// save to file
logStreamRF.Write(data, 0, data.Length);
// filter data
formMain.FilteredRoll = 0.99 * formMain.FilteredRoll + 0.01 * RelayedData.dRoll;
formMain.FilteredPitch = 0.99 * formMain.FilteredPitch + 0.01 * RelayedData.dPitch;
formMain.FilteredYaw = 0.99 * formMain.FilteredYaw + 0.01 * RelayedData.dYaw;
formMain.DisplayRelayedData(RelayedData);
}
if (type == 0x62)
{
// Parameters structure
SParameters parametersDataHopeRF = (SParameters)Comm.FromBytes(data, new SParameters());
// Display
formMain.DisplayParams(parametersDataHopeRF);
}
}
public static unsafe void ExtractLogFileRF(string filename)
{
// open txt file
StreamWriter sw = File.CreateText("matlabRF.txt");
// create header
sw.Write("Loop ");
sw.Write("Voltage CurrentA TotalCharge_mAh ");
sw.Write("Roll Pitch Yaw ");
sw.Write("dRoll dPitch dYaw ");
sw.Write("Altitude Vertspeed ");
sw.Write("NumSV VelN VelE ");
sw.Write("HopeRXRSSI HopeTXRSSI ");
sw.WriteLine("PWM1 PWM2 PWM3 PWM4 ");
SCommHopeRFDataA2Avion data = new SCommHopeRFDataA2Avion();
FileStream logStream = File.OpenRead(filename);
int size = Marshal.SizeOf(data);
byte[] dataArray = new byte[size];
while (logStream.Read(dataArray, 0, size) > 0)
{
data = (SCommHopeRFDataA2Avion)Comm.FromBytes(dataArray, new SCommHopeRFDataA2Avion());
sw.Write("{0} ", data.LoopCounter);
sw.Write("{0} {1} {2} ", data.BatteryVoltage, data.BatteryCurrentA, data.BatteryTotalCharge_mAh);
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} ", data.Altitude, data.Vertspeed);
sw.Write("{0} {1} {2} ", data.NumSV, data.VelN / 1000.0, data.VelE / 1000.0);
sw.Write("{0} {1} ", data.HopeRXRSSI, data.HopeTXRSSI);
sw.Write("{0} {1} {2} {3} ", data.MotorThrusts[0], data.MotorThrusts[1], data.MotorThrusts[2], data.MotorThrusts[3]);
sw.WriteLine("");
}
sw.Close();
}
public unsafe void DisplayRelayedData(SCommHopeRFDataA2Avion RelayedData)
{
if (radioButtonHopeRF.Checked || radioButtonWifiHopeRF.Checked)
{
// GPS
textBoxLocSatNr.Text = RelayedData.NumSV.ToString();
textBoxNavLocSatNr.Text = RelayedData.NumSV.ToString();
textBoxLocLat.Text = (RelayedData.Latitude * 1e-7).ToString("0.000000");
textBoxLocLon.Text = (RelayedData.Longitude * 1e-7).ToString("0.000000");
textBoxLocVelocityN.Text = (RelayedData.VelN / 1000.0).ToString("0.00 m/s");
textBoxLocVelocityE.Text = (RelayedData.VelE / 1000.0).ToString("0.00 m/s");
textBoxLocAlt.Text = (RelayedData.Altitude).ToString("0.0 m");
textBoxLocVertSpeed.Text = (RelayedData.Vertspeed).ToString("0.0 m/s");
// Attitude
textBoxRoll.Text = RelayedData.Roll.ToString("0.0 °");
textBoxPitch.Text = RelayedData.Pitch.ToString("0.0 °");
textBoxYaw.Text = RelayedData.Yaw.ToString("0.0 °");
textBoxdRoll.Text = RelayedData.dRoll.ToString("0.0 °/s");
textBoxdPitch.Text = RelayedData.dPitch.ToString("0.0 °/s");
textBoxdYaw.Text = RelayedData.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
textBoxSystemLoop.Text = RelayedData.LoopCounter.ToString();
textBoxSystemBattery.Text = RelayedData.BatteryVoltage.ToString("0.00 V");
textBoxSystemMode.Text = RelayedData.ActualMode.ToString();
textBoxNavigationMode.Text = RelayedData.ActualMode.ToString();
textBoxSystemBatteryCurrentA.Text = RelayedData.BatteryCurrentA.ToString("0.0 A");
textBoxSystemBatteryTotalCharge_mAH.Text = RelayedData.BatteryTotalCharge_mAh.ToString("0 mAh");
// HopeRF
textBoxHopeRXCount.Text = RelayedData.HopeRXFrameCount.ToString();
textBoxHopeRSSI.Text = RelayedData.HopeTXRSSI.ToString();
textBoxHopeRXRSSI.Text = RelayedData.HopeRXRSSI.ToString();
// Motors
textBoxSystemT1.Text = RelayedData.MotorThrusts[0].ToString();
textBoxSystemT2.Text = RelayedData.MotorThrusts[1].ToString();
textBoxSystemT3.Text = RelayedData.MotorThrusts[2].ToString();
textBoxSystemT4.Text = RelayedData.MotorThrusts[3].ToString();
// Download map
textBoxMapDownloadLat.Text = (RelayedData.Latitude * 1e-7).ToString("0.000000");
textBoxMapDownloadLon.Text = (RelayedData.Longitude * 1e-7).ToString("0.000000");
// Update current position
navigation.UpdateCurrentPosition(RelayedData.Altitude, RelayedData.Longitude * 1e-7, RelayedData.Latitude * 1e-7, RelayedData.ActualMode);
// update Params Command Ack
ParamsCommandAckCntReceived = RelayedData.ParamsCommandAckCnt;
textBoxParamsStateNum.Text = string.Format("{0}/{1}", ParamsCommandAckCntReceived, ParamsCommandAckCntExpected);
if (ParamsCommandAckCntReceived == ParamsCommandAckCntExpected)
{
textBoxParamsState.BackColor = Color.Green;
textBoxParamsState.Text = "OK";
}
else
{
textBoxParamsState.BackColor = Color.Red;
textBoxParamsState.Text = "FAIL";
}
}
}
