Esempio n. 1
0
        private void buttonWriteParams_Click(object sender, EventArgs e)
        {
            // Get data from form
            SParameters p = new SParameters();

            p.GyroOffX    = float.Parse(textBoxParam1.Text);
            p.GyroOffY    = float.Parse(textBoxParam2.Text);
            p.GyroOffZ    = float.Parse(textBoxParam3.Text);
            p.MagOffX     = float.Parse(textBoxParam4.Text);
            p.MagOffY     = float.Parse(textBoxParam5.Text);
            p.MagOffZ     = float.Parse(textBoxParam6.Text);
            p.AttOffRoll  = float.Parse(textBoxParam7.Text);
            p.AttOffPitch = float.Parse(textBoxParam8.Text);
            p.RollMax     = float.Parse(textBoxParam9.Text);
            p.RollKp      = float.Parse(textBoxParam10.Text);
            p.RollKi      = float.Parse(textBoxParam11.Text);
            p.RollKd      = float.Parse(textBoxParam12.Text);
            p.PitchMax    = float.Parse(textBoxParam13.Text);
            p.PitchKp     = float.Parse(textBoxParam14.Text);
            p.PitchKi     = float.Parse(textBoxParam15.Text);
            p.PitchKd     = float.Parse(textBoxParam16.Text);

            // Add CRC
            byte[] data = Comm.GetBytes(p);
            p.CRC32 = Crc32.CalculateCrc32(data, data.Length - sizeof(uint));

            mainSystem.WriteParams(p);
            ParamsCommandAckCntExpected = ParamsCommandAckCntReceived + 1;
        }
Esempio n. 2
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);
            }
        }
Esempio n. 3
0
        private void UpdateTrackBars(SParameters p)
        {
            int RollKp = (int)(p.RollKp / 0.1 * 20);
            int RollKi = (int)(p.RollKi / 0.02 * 20);
            int RollKd = (int)(p.RollKd / 0.02 * 20);

            int PitchKp = (int)(p.PitchKp / 0.1 * 20);
            int PitchKi = (int)(p.PitchKi / 0.02 * 20);
            int PitchKd = (int)(p.PitchKd / 0.02 * 20);

            trackBarParamsRollKp.Value = RollKp;
            trackBarParamsRollKi.Value = RollKi;
            trackBarParamsRollKd.Value = RollKd;

            trackBarParamsPitchKp.Value = PitchKp;
            trackBarParamsPitchKi.Value = PitchKi;
            trackBarParamsPitchKd.Value = PitchKd;
        }
Esempio n. 4
0
        public void DisplayParams(SParameters p)
        {
            textBoxParam1.Text  = p.GyroOffX.ToString();
            textBoxParam2.Text  = p.GyroOffY.ToString();
            textBoxParam3.Text  = p.GyroOffZ.ToString();
            textBoxParam4.Text  = p.MagOffX.ToString();
            textBoxParam5.Text  = p.MagOffY.ToString();
            textBoxParam6.Text  = p.MagOffZ.ToString();
            textBoxParam7.Text  = p.AttOffRoll.ToString();
            textBoxParam8.Text  = p.AttOffPitch.ToString();
            textBoxParam9.Text  = p.RollMax.ToString();
            textBoxParam10.Text = p.RollKp.ToString();
            textBoxParam11.Text = p.RollKi.ToString();
            textBoxParam12.Text = p.RollKd.ToString();
            textBoxParam13.Text = p.PitchMax.ToString();
            textBoxParam14.Text = p.PitchKp.ToString();
            textBoxParam15.Text = p.PitchKi.ToString();
            textBoxParam16.Text = p.PitchKd.ToString();

            UpdateTrackBars(p);
        }
Esempio n. 5
0
        ///////////////////////////////
        // 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);
            }
        }
Esempio n. 6
0
 public void WriteParams(SParameters p)
 {
     // Send
     byte[] toSend = Comm.GetBytes(p);
     formMain.SendData(0x60, toSend);
 }