float theta = 35 + 90; //角度值 4f->64 private void check_uwb(byte Response1, byte Response2, int Check) //02,10,F3,02,4F,4B,E5, { RX_Counter = 0; theta = Convert.ToInt32(textBox2.Text) + 90; label32.Text = Convert.ToString(theta); double radian = theta * Math.PI / 180;//轉換弧度值 //Thread.Sleep(1300); //label10.Text = Response[0].ToString("X2") + " " + Response[1].ToString("X2") + " "; if (Response[0] == Response1 && Response[1] == Response2) { label9.Text = "UWB OK"; /* * label10.Text = Response[0].ToString("X2") + " " + Response[24].ToString("X2") + " " + Response[25].ToString("X2") + " " + Response[26].ToString("X2") + " "; * float Tag_dis1 = ((Response[26] << 16) | (Response[25] << 8) | (Response[24] << 0)) / 256000.0f; * label12.Text = Convert.ToString(Tag_dis1);*/ Anchor_x = ((Response[6] << 24) | (Response[5] << 16) | (Response[4] << 8) | (0x00 << 0)) / 256000.0f; label26.Text = Convert.ToString(Anchor_x); Anchor_y = ((Response[9] << 24) | (Response[8] << 16) | (Response[7] << 8) | (0x00 << 0)) / 256000.0f; label27.Text = Convert.ToString(Anchor_y); Anchor_z = ((Response[12] << 24) | (Response[11] << 16) | (Response[10] << 8) | (0x00 << 0)) / 256000.0f; //Anchor_z = Anchor_z + 0.3; label28.Text = Convert.ToString(Anchor_z); if (Anchor_z < 1) { Anchor_z = Anchor_z + 0.2; //4f-> 0.3 } //processFrameData(); if (send_flag) { long cur_ms = stopwatch.ElapsedMilliseconds; MAVLink.mavlink_att_pos_mocap_t att_pos = new MAVLink.mavlink_att_pos_mocap_t(); //att_pos.time_usec = (ulong)(((DateTime.UtcNow - unix_epoch).TotalMilliseconds - 10) * 1000); att_pos.time_usec = (ulong)(cur_ms * 1000); //att_pos.x = Anchor_y; //north Anchor_y //att_pos.y = Anchor_x; //east Anchor_x att_pos.x = (float)(Anchor_x * Math.Cos(radian) + Anchor_y * Math.Sin(radian)); //north att_pos.y = (float)-((-Anchor_x * Math.Sin(radian)) + Anchor_y * Math.Cos(radian)); //east att_pos.z = (float)-Anchor_z; //down Anchor_x = att_pos.x; Anchor_y = att_pos.y; Anchor_z = att_pos.z; label7.Text = Convert.ToString(att_pos.x); label13.Text = Convert.ToString(att_pos.y); label22.Text = Convert.ToString(att_pos.z); //att_pos.q = new float[4] { rbData.qw, rbData.qx, rbData.qz, -rbData.qy }; DroneData drone = drones["bebop2"]; drone.lost_count = 0; byte[] pkt; pkt = mavlinkParse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.ATT_POS_MOCAP, att_pos); mavSock.SendTo(pkt, drone.ep); } // for (int i = 0; i < 23; i++ ) // richTextBox1.Text += Response[i].ToString("X2") + " "; if (log_flag) { Log_Counter = Log_Counter + 1; try { wSheet = (Excel._Worksheet)wBook.Worksheets[1]; // 引用第一個工作表 wSheet.Name = "UWB Sensor Value Log"; // 命名工作表的名稱 wSheet.Activate(); // 設定工作表焦點 //excelApp.Cells[1, 1] = "Excel測試"; excelApp.Cells[Log_Counter, 1] = Anchor_x; //att_pos.x excelApp.Cells[Log_Counter, 2] = Anchor_y; //att_pos.y excelApp.Cells[Log_Counter, 3] = Anchor_z; //att_pos.z } catch (Exception ex) { Console.WriteLine("產生報表時出錯!" + Environment.NewLine + ex.Message); } if (log_flag_set) { try { //另存活頁簿 wBook.SaveAs(pathFile, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Excel.XlSaveAsAccessMode.xlNoChange, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Type.Missing); Console.WriteLine("儲存文件於 " + Environment.NewLine + pathFile); } catch (Exception ex) { Console.WriteLine("儲存檔案出錯,檔案可能正在使用" + Environment.NewLine + ex.Message); } wBook.Close(false, Type.Missing, Type.Missing); //關閉活頁簿 excelApp.Quit(); //關閉Excel System.Runtime.InteropServices.Marshal.ReleaseComObject(excelApp); //釋放Excel資源 wBook = null; wSheet = null; excelApp = null; GC.Collect(); Console.Read(); log_flag = false; log_flag_set = false; Log_Counter = 0; } } } }
//static uint GPS_LEAPSECONDS_MILLIS = 18000; static void processFrameData(NatNetML.FrameOfMocapData data) { bool data_gogo = true; /* Parsing Rigid Body Frame Data */ for (int i = 0; i < mRigidBodies.Count; i++) { int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions for (int j = 0; j < data.nRigidBodies; j++) { if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data. { NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions if (rbData.Tracked == true) { #if DEBUG_MSG Console.WriteLine("\tRigidBody ({0}):", rb.Name); Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z); // Rigid Body Euler Orientation float[] quat = new float[4] { rbData.qx, rbData.qy, rbData.qz, rbData.qw }; float[] eulers = new float[3]; eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation. double xrot = RadiansToDegrees(eulers[0]); double yrot = RadiansToDegrees(eulers[1]); double zrot = RadiansToDegrees(eulers[2]); Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot); #endif if (drones.ContainsKey(rb.Name)) { DroneData drone = drones[rb.Name]; drone.lost_count = 0; long cur_ms = stopwatch.ElapsedMilliseconds; if (drone.send_count > 0) { drone.send_count--; } else if (data_gogo) { drone.send_count = 10; data_gogo = false; MAVLink.mavlink_att_pos_mocap_t att_pos = new MAVLink.mavlink_att_pos_mocap_t(); att_pos.time_usec = (ulong)(cur_ms * 1000); att_pos.x = rbData.x; //north att_pos.y = rbData.z; //east att_pos.z = -rbData.y; //down att_pos.q = new float[4] { rbData.qw, rbData.qx, rbData.qz, -rbData.qy }; byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.ATT_POS_MOCAP, att_pos); if (drone.lastTime >= 0) { MAVLink.mavlink_vision_speed_estimate_t vis_speed = new MAVLink.mavlink_vision_speed_estimate_t(); float total_s = (cur_ms - drone.lastTime) * 0.001f; vis_speed.x = (rbData.x - drone.lastPosN) / total_s; vis_speed.y = (rbData.z - drone.lastPosE) / total_s; vis_speed.z = (-rbData.y - drone.lastPosD) / total_s; vis_speed.usec = (ulong)(cur_ms * 1000); byte[] pkt2 = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.VISION_SPEED_ESTIMATE, vis_speed); int total_len = pkt.Length + pkt2.Length; byte[] uwb_data = new byte[10 + total_len + 1]; uwb_data[0] = 0x54; uwb_data[1] = 0xf1; uwb_data[2] = 0xff; uwb_data[3] = 0xff; uwb_data[4] = 0xff; uwb_data[5] = 0xff; uwb_data[6] = 2; uwb_data[7] = drone.uwb_tag_id; uwb_data[8] = (byte)(total_len & 0xff); uwb_data[9] = (byte)(total_len >> 16); Array.Copy(pkt, 0, uwb_data, 10, pkt.Length); Array.Copy(pkt2, 0, uwb_data, 10 + pkt.Length, pkt2.Length); byte chk_sum = 0; foreach (byte uwb_data_byte in uwb_data) { chk_sum += uwb_data_byte; } uwb_data[uwb_data.Length - 1] = chk_sum; mavSock.SendTo(uwb_data, drone.ep); } } drone.lastTime = cur_ms; drone.lastPosN = rbData.x; drone.lastPosE = rbData.z; drone.lastPosD = -rbData.y; } } else { Console.WriteLine("\t{0} is not tracked in current frame", rb.Name); /*if (drones.ContainsKey(rb.Name)) * { * DroneData drone = drones[rb.Name]; * drone.lost_count++; * if (drone.lost_count > 3) * { * drone.lost_count = 0; * MAVLink.mavlink_gps_input_t gps_input = new MAVLink.mavlink_gps_input_t(); * gps_input.gps_id = 0; * gps_input.fix_type = (byte)MAVLink.GPS_FIX_TYPE.NO_FIX; * byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.GPS_INPUT, gps_input); * mavSock.SendTo(pkt, drone.ep); * } * }*/ } } } } /* Parsing Skeleton Frame Data */ for (int i = 0; i < mSkeletons.Count; i++) // Fetching skeleton IDs from the saved descriptions { int sklID = mSkeletons[i].ID; for (int j = 0; j < data.nSkeletons; j++) { if (sklID == data.Skeletons[j].ID) // When skeleton ID of the description matches skeleton ID of the frame data. { NatNetML.Skeleton skl = mSkeletons[i]; // Saved skeleton descriptions NatNetML.SkeletonData sklData = data.Skeletons[j]; // Received skeleton frame data Console.WriteLine("\tSkeleton ({0}):", skl.Name); Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies); /* Now, for each of the skeleton segments */ for (int k = 0; k < sklData.nRigidBodies; k++) { NatNetML.RigidBodyData boneData = sklData.RigidBodies[k]; /* Decoding skeleton bone ID */ int skeletonID = HighWord(boneData.ID); int rigidBodyID = LowWord(boneData.ID); int uniqueID = skeletonID * 1000 + rigidBodyID; int key = uniqueID.GetHashCode(); NatNetML.RigidBody bone = (RigidBody)mHtSkelRBs[key]; //Fetching saved skeleton bone descriptions //Outputting only the hip segment data for the purpose of this sample. if (k == 0) { Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z); } } } } } /* Parsing Force Plate Frame Data */ for (int i = 0; i < mForcePlates.Count; i++) { int fpID = mForcePlates[i].ID; // Fetching force plate IDs from the saved descriptions for (int j = 0; j < data.nForcePlates; j++) { if (fpID == data.ForcePlates[j].ID) // When force plate ID of the descriptions matches force plate ID of the frame data. { NatNetML.ForcePlate fp = mForcePlates[i]; // Saved force plate descriptions NatNetML.ForcePlateData fpData = data.ForcePlates[i]; // Received forceplate frame data Console.WriteLine("\tForce Plate ({0}):", fp.Serial); // Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame. for (int k = 0; k < fpData.nChannels; k++) { Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]); } } } } #if DEBUG_MSG Console.WriteLine("\n"); #endif }