private void ProcessReadings(WifiPacket packet)
{
readingsToProcess.Enqueue (new WifiReading{ timestamp_us = packet.timestamp_us, signal_dbm = packet.signal_dbm });
bool not_in_history, not_yet;
Matrix4x4 robotToGlobal = new Matrix4x4();
while (readingsToProcess.Count > 0)
{
WifiReading reading = readingsToProcess.Peek ();
PositionHistory.PositionSnapshot snapshot= positionHistory.GetPositionSnapshotThreadSafe(reading.timestamp_us, reading.timestamp_us, out not_yet, out not_in_history);
if (not_in_history)
{
print("wifi - ignoring packet (position data not in history) with timestamp " + reading.timestamp_us);
readingsToProcess.Dequeue ();
continue;
}
if (not_yet) //wait for position data to arrive
return;
readingsToProcess.Dequeue();
float height01 = ((float)reading.signal_dbm - (float)signal.minValueDbm) / ((float)signal.maxValueDbm-(float)signal.minValueDbm);
float heightm = Mathf.LerpUnclamped(plot.minHeight, plot.maxHeight, height01);
Vector3 position = new Vector3(wifiPosition.x, heightm, wifiPosition.z);
PositionData pos = snapshot.PositionAt (reading.timestamp_us);
robotToGlobal.SetTRS(pos.position, Quaternion.Euler(0.0f, pos.heading, 0.0f), Vector3.one);
position = robotToGlobal.MultiplyPoint3x4(position);
wifiReadingsCB.Put (position);
}
}
private bool TranslateReadingsToGlobalReferenceFrame(ref int from,ref int len, ulong t_from, ulong t_to)
{
bool not_in_history, not_yet;
if (threadInternal.pending && t_from != threadInternal.t_from)
AddPendingDataToProcess(ref from, ref len, ref t_from);
PositionHistory.PositionSnapshot snapshot= positionHistory.GetPositionSnapshotThreadSafe(t_from, t_to, out not_yet, out not_in_history);
if (not_in_history)
{
print("Laser - ignoring packet (position data not in history) with timestamp " + t_from);
threadInternal.pending = false;
return false;
}
if (not_yet)
{
threadInternal.SetPending (from, len, t_from, t_to);
return false;
}
threadInternal.pending = false;
Matrix4x4 robotToGlobal = new Matrix4x4();
Vector3 scale = Vector3.one;
PositionData pos=new PositionData();
ulong[] timestamps = threadInternal.timestamps;
Vector3[] readings = threadInternal.readings;
for (int i = from, ind; i < from+len; ++i)
{
ind = i % 360;
pos = snapshot.PositionAt(timestamps[ind]);
robotToGlobal.SetTRS(pos.position, Quaternion.Euler(0.0f, pos.heading, 0.0f), scale);
readings[ind]=robotToGlobal.MultiplyPoint3x4(readings[ind]);
}
return true;
}
