Exemplo n.º 1
0
        ///////////////////////////////
        // 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);
            }
        }
Exemplo n.º 2
0
        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();
        }
Exemplo n.º 3
0
        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";
                }
            }
        }