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