/** * Modifies packet by reference to add its distance to the current samples * and calculate the filtered distance by weighted average. */ public void ProcessPacket(AnchorInfoPacket packet) { // Tune value based on offset packet.distance += RangeOffset; //Maintain reference to most recent packet lastPacket = packet; samples.Enqueue(packet.distance); packet.distance_filtered = FindWeightedAverage(packet.distance); }
//Method to be called when broadcasts are received public void ReceiveBroadcast(TagPacket receivedPacket) { print("Received Broadcast"); Vector3 senderPosition = receivedPacket.position; float timeDelay = GetTimeDelay(senderPosition); distanceToTag = Vector3.Distance(senderPosition, transform.position); AnchorInfoPacket packetToSend = new AnchorInfoPacket(transform.gameObject.name, receivedPacket.tagID, receivedPacket.blinkID, transform.position, //position of this anchor senderPosition, //position of tag timeDelay); //TDOA value OnPacketSent(packetToSend); }
public void ProcessRangeReport(string tagId, List <TcpServer.AnchorRange> ranges) { if (textComponent != null) { textComponent.text = DateTime.Now.ToLongTimeString(); } // Build and process all packets based on ranges List <AnchorInfoPacket> packets = new List <AnchorInfoPacket>(); foreach (TcpServer.AnchorRange r in ranges) { float d = (float)r.dist; AnchorInfoPacket packet = new AnchorInfoPacket(); packet.anchorID = r.id; packet.blinkID = 1; packet.tagID = tagId; packet.timeDelay = d / Constants.SPEED_OF_LIGHT; packet.distance = d; try { packet.anchorLocation = GameObject.FindGameObjectsWithTag(TagDefs.ANCHOR_TAG).First(obj => obj.name == packet.anchorID).transform.position; } catch (Exception) { Debug.LogError("Unknown anchor id: " + packet.anchorID); } GameObject anchorObject = (GameObject)GameObject.Find(packet.anchorID); packet.anchorScript = anchorObject.GetComponent <AnchorScript>(); packet.anchorScript.ProcessPacket(packet); packets.Add(packet); } // Gets the 3 closest packets List <AnchorInfoPacket> closestPackets = new List <AnchorInfoPacket> (); packets.Sort((a, b) => a.distance.CompareTo(b.distance)); closestPackets = packets.Take(3).ToList <AnchorInfoPacket>(); // Toggle IsRanging packets.ForEach((AnchorInfoPacket obj) => obj.anchorScript.IsRanging = false); closestPackets.ForEach((AnchorInfoPacket obj) => obj.anchorScript.IsRanging = true); // Update the position of the tag UpdateTag(tagId, closestPackets); }
public void ProcessPacket(AnchorInfoPacket packet) { lastPacket = packet; samples.Enqueue(packet.distance); if (samples.Count >= SampleSize) { samples.Dequeue(); float accum = 0; float[] samplesArray = samples.ToArray(); for (int i = 0; i < samplesArray.Length; i++) { accum += samplesArray[i]; } float avg = accum / samplesArray.Length; packet.distance_filtered = avg; } else { packet.distance_filtered = packet.distance; } }
void Update() { AnchorInfoPacket packet = anchorScript.GetLastAnchorInfoPacket(); Vector3 ab = Tag.transform.position - Anchor.transform.position; float dist = ab.magnitude; ab.Normalize(); Vector3 pos = Anchor.transform.position + ((dist * 0.5f) * ab); pos.y = 0.5f; textMesh.transform.position = pos; if (packet != null) { textMesh.text = packet.distance_filtered.ToString("#0.00 m"); } else { textMesh.text = "0.00 m"; } }
public void ReceivePacket(AnchorInfoPacket packet) { anchorPackets.Add(packet); print("New Packet"); }