/**
* 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");
}
