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 { } }