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