void time1_Tick(object sender, EventArgs e)
{
angle += 1;
Quaternion q = ZRotation(angle * Math.PI / 180.0);
view3D.SetQuaternion(q.X, q.Y, q.Z, q.W);
}
void time1_Tick(object sender, EventArgs e)
{
view3D.SetQuaternion(qRotation.X, qRotation.Y, qRotation.Z, qRotation.W);
label1.Text = "四元数 W X Y Z:" + qRotation.W.ToString("f3") + " " + qRotation.X.ToString("f3") + " " + qRotation.Y.ToString("f3") + " " + qRotation.Z.ToString("f3");
}
/// <summary>
/// 接收数据
/// </summary>
private void ReceiveMsg()
{
Console.WriteLine("线程启动");
// UdpClient UCR;
try
{
// UCR = new UdpClient(ReceivePort);
// UCR = new UdpClient(9002);
//取0可接收其它端口的数据，这里上面new UdpClient(SendPort)的设计好象没有起到应有的作用。
//IPEndPoint Sender = new IPEndPoint(IPAddress.Parse("127.0.0.1"), 9000);
IPEndPoint Sender = new IPEndPoint(IPAddress.Any, 0);
while (true)
{
try
{
//IPEndPoint Sender = null;
//Receive处于等待，但不影响发送
byte[] data = new byte[1024];
data = UCR.Receive(ref Sender);
if (state == true)
{
SendMsg(ref d, UCR);
}
Console.WriteLine("{0},{1}", data.Length, BitConverter.ToString(data));//打印数据流
if (data.Length == 0)
{
break;
}
DataService ds = new DataService();
int cursor = 0;//游标指针
int copylength = 0;
int length = 0;
for (int i = 0; i < data.Length; i += length)
{
cursor = cursor + 1;
if ((data[i] == 0xaa) && (data[i + 1] == 0xaa))
{
int len = int.Parse(Convert.ToString(data[i + 3], 10));//数据正文长度
cursor = cursor + 3;
//减1的目的是保证数据在【有正文的时候，还有一位是和校验位】
if (data.Length - cursor > len)
{
cursor = cursor + len;//完整的数据帧【除去和校验位】
byte[] df = new byte[len + 4];
for (int j = 0; j < len + 4; j++)
{
df[j] = data[j + copylength];//取出数据
}
if (ds.CheckSum(df) == data[cursor])
{
byte funcode = data[i + 2];
#region 显示数据
switch (funcode)
{
case 0x00:
break;
case 0x01:
//修改
this.Invoke(new Action(() =>
{
short temp = 0; //一个16位整形变量，初值为 0000 0000 0000 0000
temp = (short)(temp ^ df[4]); //低8位
temp = (short)(temp << 8); //高8位
temp = (short)(temp ^ df[5]); //在b2赋给s的低8位
lab_roll.Text = ((float)temp / 100).ToString();
short temp2 = 0;
temp2 = (short)(temp2 ^ df[6]); //低8位
temp2 = (short)(temp2 << 8); //高8位
temp2 = (short)(temp2 ^ df[7]); //在b2赋给s的低8位
lab_pitch.Text = ((float)temp2 / 100).ToString();
short temp3 = 0;
temp3 = (short)(temp3 ^ df[8]); //低8位
temp3 = (short)(temp3 << 8); //高8位
temp3 = (short)(temp3 ^ df[9]); //在b2赋给s的低8位
lab_yaw.Text = ((float)temp3 / 100).ToString();
int pow = 0;
pow = (short)(pow ^ df[12]); //低8位
pow = (short)(pow << 8); //高8位
pow = (short)(pow ^ df[13]); //在b2赋给s的低8位
lab_butter.Text = (((float)(pow)) / 100).ToString();
double x, y, z, w;
w = Math.Cos(temp / 200 * 0.0174533) * Math.Cos(temp2 / 200 * 0.0174533) * Math.Cos(temp3 / 200 * 0.0174533) + Math.Sin(temp / 200 * 0.0174533) * Math.Sin(temp2 / 200 * 0.0174533) * Math.Sin(temp3 / 200 * 0.0174533);
x = Math.Sin(temp / 200 * 0.0174533) * Math.Cos(temp2 / 200 * 0.0174533) * Math.Cos(temp3 / 200 * 0.0174533) - Math.Cos(temp / 200 * 0.0174533) * Math.Sin(temp2 / 200 * 0.0174533) * Math.Sin(temp3 / 200 * 0.0174533);
y = Math.Cos(temp / 200 * 0.0174533) * Math.Sin(temp2 / 200 * 0.0174533) * Math.Cos(temp3 / 200 * 0.0174533) + Math.Sin(temp / 200 * 0.0174533) * Math.Cos(temp2 / 200 * 0.0174533) * Math.Sin(temp3 / 200 * 0.0174533);
z = Math.Cos(temp / 200 * 0.0174533) * Math.Cos(temp2 / 200 * 0.0174533) * Math.Sin(temp3 / 200 * 0.0174533) - Math.Sin(temp / 200 * 0.0174533) * Math.Sin(temp2 / 200 * 0.0174533) * Math.Cos(temp3 / 200 * 0.0174533);
view3D.SetQuaternion(x, y, z, w);
if (df[15] == 0x01)
{
lab_lock.Text = "解锁";
}
else if (df[15] == 0x00)
{
lab_lock.Text = "加锁";
}
}));
break;
case 0x02:
//9.80665/32767
short temp4 = 0;
temp4 = (short)(temp4 ^ df[4]); //低8位
temp4 = (short)(temp4 << 8); //高8位
temp4 = (short)(temp4 ^ df[5]); //在b2赋给s的低8位
lab_acc_x.Text = temp4.ToString();
short temp5 = 0;
temp5 = (short)(temp5 ^ df[6]); //低8位
temp5 = (short)(temp5 << 8); //高8位
temp5 = (short)(temp5 ^ df[7]); //在b2赋给s的低8位
lab_acc_y.Text = temp5.ToString();
short temp6 = 0; //一个16位整形变量，初值为 0000 0000 0000 0000
temp6 = (short)(temp6 ^ df[8]); //低8位
temp6 = (short)(temp6 << 8); //高8位
temp6 = (short)(temp6 ^ df[9]); //在b2赋给s的低8位
lab_acc_z.Text = temp6.ToString();
//lab_acc_z.Text = ( (double)temp6* 9.80665 / 32767).ToString();
//gyr
short temp7 = 0; //一个16位整形变量，初值为 0000 0000 0000 0000
temp7 = (short)(temp7 ^ df[10]); //低8位
temp7 = (short)(temp7 << 8); //高8位
temp7 = (short)(temp7 ^ df[11]); //在b2赋给s的低8位
lab_gyrd_x.Text = temp7.ToString();
short temp8 = 0; //一个16位整形变量，初值为 0000 0000 0000 0000
temp8 = (short)(temp8 ^ df[12]); //低8位
temp8 = (short)(temp8 << 8); //高8位
temp8 = (short)(temp8 ^ df[13]); //在b2赋给s的低8位
lab_gyrd_y.Text = temp8.ToString();
short temp9 = 0; //一个16位整形变量，初值为 0000 0000 0000 0000
temp9 = (short)(temp9 ^ df[14]); //低8位
temp9 = (short)(temp9 << 8); //高8位
temp9 = (short)(temp9 ^ df[15]); //在b2赋给s的低8位
lab_gyrd_z.Text = temp9.ToString();
//
//lab_gyrd_x.Text = (((df[10] << 8) + df[11]) ).ToString();
//lab_gyrd_y.Text = (((df[12] << 8) + df[13]) ).ToString();
//lab_gyrd_z.Text = (((df[14] << 8) + df[15]) ).ToString();
short temp10 = 0; //一个16位整形变量，初值为 0000 0000 0000 0000
temp10 = (short)(temp10 ^ df[16]); //低8位
temp10 = (short)(temp10 << 8); //高8位
temp10 = (short)(temp10 ^ df[17]); //在b2赋给s的低8位
lab_mag_x.Text = temp10.ToString();
short temp11 = 0; //一个16位整形变量，初值为 0000 0000 0000 0000
temp11 = (short)(temp11 ^ df[18]); //低8位
temp11 = (short)(temp11 << 8); //高8位
temp11 = (short)(temp11 ^ df[19]); //在b2赋给s的低8位
lab_mag_y.Text = temp11.ToString();
short temp12 = 0; //一个16位整形变量，初值为 0000 0000 0000 0000
temp12 = (short)(temp12 ^ df[20]); //低8位
temp12 = (short)(temp12 << 8); //高8位
temp12 = (short)(temp12 ^ df[21]); //在b2赋给s的低8位
lab_mag_z.Text = temp12.ToString();
this.FilghtControlFrm_Load(null, null); //刷新
break;
case 0x03:
txt_thr.Text = ((int)(((df[4] << 8) + df[5]))).ToString();
txt_yaw.Text = ((int)(((df[6] << 8) + df[7]))).ToString();
txt_roll.Text = ((int)(((df[8] << 8) + df[9]))).ToString();
txt_pitch.Text = ((int)(((df[10] << 8) + df[11]))).ToString();
short tempa = 0;
tempa = (short)(tempa ^ df[12]);
tempa = (short)(tempa << 8);
tempa = (short)(tempa ^ df[13]);
txt_pid_7_rol.Text = tempa.ToString();
short tempb = 0;
tempb = (short)(tempb ^ df[14]);
tempb = (short)(tempb << 8);
tempb = (short)(tempb ^ df[15]);
txt_pid_8_rol.Text = tempb.ToString();
short tempc = 0;
tempc = (short)(tempc ^ df[16]);
tempc = (short)(tempc << 8);
tempc = (short)(tempc ^ df[17]);
txt_pid_9_rol.Text = tempc.ToString();
short tempd = 0;
tempd = (short)(tempd ^ df[18]);
tempd = (short)(tempd << 8);
tempd = (short)(tempd ^ df[19]);
txt_pid_7_yaw.Text = tempd.ToString();
short tempe = 0;
tempe = (short)(tempe ^ df[20]);
tempe = (short)(tempe << 8);
tempe = (short)(tempe ^ df[21]);
txt_pid_8_yaw.Text = tempe.ToString();
short tempf = 0;
tempf = (short)(tempf ^ df[22]);
tempf = (short)(tempf << 8);
tempf = (short)(tempf ^ df[23]);
txt_pid_9_yaw.Text = tempf.ToString();
break;
case 0x06:
break;
case 0x10:
txt_pid_1_rol.Text = (((df[4] << 8) + df[5])).ToString();
txt_pid_2_rol.Text = (((df[6] << 8) + df[7])).ToString();
txt_pid_3_rol.Text = (((df[8] << 8) + df[9])).ToString();
txt_pid_1_yaw.Text = (((df[10] << 8) + df[11])).ToString();
txt_pid_2_yaw.Text = (((df[12] << 8) + df[13])).ToString();
txt_pid_3_yaw.Text = (((df[14] << 8) + df[15])).ToString();
txt_pid_1_pitch.Text = (((df[16] << 8) + df[17])).ToString();
txt_pid_2_pitch.Text = (((df[18] << 8) + df[19])).ToString();
txt_pid_3_pitch.Text = (((df[20] << 8) + df[21])).ToString();
break;
case 0x11:
txt_pid_4_rol.Text = (((df[4] << 8) + df[5])).ToString();
txt_pid_5_rol.Text = (((df[6] << 8) + df[7])).ToString();
txt_pid_6_rol.Text = (((df[8] << 8) + df[9])).ToString();
txt_pid_4_yaw.Text = (((df[10] << 8) + df[11])).ToString();
txt_pid_5_yaw.Text = (((df[12] << 8) + df[13])).ToString();
txt_pid_6_yaw.Text = (((df[14] << 8) + df[15])).ToString();
txt_pid_4_pitch.Text = (((df[16] << 8) + df[17])).ToString();
txt_pid_5_pitch.Text = (((df[18] << 8) + df[19])).ToString();
txt_pid_6_pitch.Text = (((df[20] << 8) + df[21])).ToString();
break;
case 0x12:
txt_pid_7_rol.Text = (((df[4] << 8) + df[5])).ToString();
txt_pid_8_rol.Text = (((df[6] << 8) + df[7])).ToString();
txt_pid_9_rol.Text = (((df[8] << 8) + df[9])).ToString();
txt_pid_7_yaw.Text = (((df[10] << 8) + df[11])).ToString();
txt_pid_8_yaw.Text = (((df[12] << 8) + df[13])).ToString();
txt_pid_9_yaw.Text = (((df[14] << 8) + df[15])).ToString();
txt_pid_7_pitch.Text = (((df[16] << 8) + df[17])).ToString();
txt_pid_8_pitch.Text = (((df[18] << 8) + df[19])).ToString();
txt_pid_9_pitch.Text = (((df[20] << 8) + df[21])).ToString();
break;
case 0xEF:
break;
default:
break;
}
#endregion
cursor = cursor + 1;
copylength = copylength + len + 5;
length = len + 5;
}
else
{
lab_msg.Text = "和校验错误！";
lab_msg.ForeColor = Color.Red;
}
}
else
{
lab_msg.Text = "数据不完整！";
lab_msg.ForeColor = Color.Red;
break;
}
}
else
{
lab_msg.Text = "数据报错误！";
lab_msg.ForeColor = Color.Red;
break;
}
}
}
catch (Exception s)
{
Console.WriteLine(s.ToString());
}
}
}
catch
{ }
}
