static void vJoyDataPack(float ROLL, float PITCH, float YAW, float THROTTLE, Frame thisFrame, Form1 sender)
{
//Display raw values
sender.rollTXT.Text = ROLL.ToString();
sender.pitchTXT.Text = PITCH.ToString();
sender.yawTXT.Text = YAW.ToString();
sender.heightTXT.Text = THROTTLE.ToString();
//Display raw values
sender.rollJOY.Text = RX.ToString();
sender.pitchJOY.Text = RY.ToString();
sender.yawJOY.Text = X.ToString();
sender.heightJOY.Text = Y.ToString();
sender.yawProg.Value = (int)X;
sender.rollProg.Value = (int)RX;
sender.pitchProg.Value = (int)RY;
sender.heightProg.Value = (int)Y;
HandValue thisHand = new HandValue();
thisHand.Roll = RX;
thisHand.Pitch = RY;
thisHand.Yaw = X;
thisHand.Height = Y;
thisHand.Timestamp = thisFrame.Timestamp;
myLog.LogHand(thisHand);
if (sender.chartCheckBox.Checked)
{
if ((thisFrame.Id % 3) == 0)
{
myCircularBuff.Add(thisHand);
}
}
}
static void serialCommData_pack(float ROLL, float PITCH, float YAW, float THROTTLE, Frame thisFrame, Form1 sender)
{
if (sender.serialPort1.IsOpen)
{
//Display raw values
sender.rollTXT.Text = ROLL.ToString();
sender.pitchTXT.Text = PITCH.ToString();
sender.yawTXT.Text = YAW.ToString();
sender.heightTXT.Text = THROTTLE.ToString();
//Display raw values
sender.rollJOY.Text = RX.ToString();
sender.pitchJOY.Text = RY.ToString();
sender.yawJOY.Text = X.ToString();
sender.heightJOY.Text = Y.ToString();
sender.yawProg.Value = (int)X;
sender.rollProg.Value = (int)RX;
sender.pitchProg.Value = (int)RY;
sender.heightProg.Value = (int)Y;
HandValue thisHand = new HandValue();
thisHand.Roll = RX;
thisHand.Pitch = RY;
thisHand.Yaw = X;
thisHand.Height = Y;
thisHand.Timestamp = thisFrame.Timestamp;
if (ball_motion)
{
int Roll_to_send = (int)(((float)(RX >> 7)) / 1.422f);
int Pitch_to_send = (int)(((float)(RY >> 7)) / 1.422f);
dataOUT[0] = (byte)'$';
dataOUT[1] = (byte)'Y';
dataOUT[2] = (byte)(((Roll_to_send % 1000) / 100) + 48);
dataOUT[3] = (byte)(((Roll_to_send % 100) / 10) + 48);
dataOUT[4] = (byte)((Roll_to_send % 10) + 48);
dataOUT[5] = (byte)'#';
dataOUT[6] = (byte)'$';
dataOUT[7] = (byte)'X';
dataOUT[8] = (byte)(((Pitch_to_send % 1000) / 100) + 48);
dataOUT[9] = (byte)(((Pitch_to_send % 100) / 10) + 48);
dataOUT[10] = (byte)((Pitch_to_send % 10) + 48);
dataOUT[11] = (byte)'#';
}
else
{
//scrittura nella seriale
dataOUT[0] = (byte)'$';
dataOUT[1] = 85;
uint Roll_to_send = (uint)((float)((uint)RX >> 5) / 1.28f); // CH1
uint Pitch_to_send = (uint)((float)((uint)RY >> 5) / 1.28f);
uint Height_to_send = (uint)((float)((uint)Y >> 5) / 1.28f);
uint Yaw_to_send = (uint)((float)((uint)X >> 5) / 1.28f);
uint arm_disarm = 800;
uint Air_mode = 0;
uint Flight_mode = (uint)sender.FlightMode;
uint CH8 = 0;
if (AirMode)
{
Air_mode = 800;
}
else
{
Air_mode = 0;
}
if (run)
{
arm_disarm = 0; // armed
}
else
{
arm_disarm = 800; // disarmed
}
dataOUT[2] = (byte)(Roll_to_send >> 2);
dataOUT[3] = (byte)((Roll_to_send << 6) | (Pitch_to_send >> 4));
dataOUT[4] = (byte)((Pitch_to_send << 4) | (Height_to_send >> 6));
dataOUT[5] = (byte)((Height_to_send << 2) | (Yaw_to_send >> 8));
dataOUT[6] = (byte)(Yaw_to_send);
dataOUT[7] = (byte)(arm_disarm >> 2);
dataOUT[8] = (byte)((arm_disarm << 6) | (Air_mode >> 4));
dataOUT[9] = (byte)((Air_mode << 4) | (Flight_mode >> 6));
dataOUT[10] = (byte)((Flight_mode << 2) | (CH8 >> 8));
dataOUT[11] = (byte)(CH8);
dataOUT[12] = (byte)'#';
}
}
}
