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