using System; using System.Linq; using System.Threading.Tasks; using Plotting; using UnityEngine; using UnityEngine.Serialization; namespace Sensors.Polar { [System.Serializable] public struct PolarSensorConfig { public int port; public String ipAddress; public bool plotAcc; public bool plotEcg; public int accSampleRate; //TODO: let user choose between 25, 50, 100, 200 public PolarSensorConfig(int port = 9099, string ipAddress = "0.0.0.0", bool plotAcc = false, bool plotEcg = false, int accSampleRate = 25) { this.port = port; this.ipAddress = ipAddress; this.plotAcc = plotAcc; this.plotEcg = plotEcg; this.accSampleRate = accSampleRate; } } public struct PolarSensorData { public Vector3 Acc; public float EcgValue; } public struct TimeSync { public DateTime StartTime; public long DifStartAccStart; public long DifStartEcgStart; } public class PolarReceiver { private UdpConnection connection; private PolarSensorData sensorData; public PolarSensorConfig SensorConfig { get; private set; } public PolarSensorData SensorData => sensorData; private PlotFileWriter ecgPlotFile; private PlotFileWriter accPlotFile; private static long startTimeAcc = -1; private static long startTimeEcg = -1; private static TimeSync timeSync; private const int ECG_SAMPLE_RATE = 130; [RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.BeforeSceneLoad)] private static void InitTime() { timeSync = new TimeSync {StartTime = DateTime.Now, DifStartAccStart = -1, DifStartEcgStart = -1}; } public PolarReceiver(PolarSensorConfig config) { SensorConfig = config; } public async Task StartListening() { if (SensorConfig.plotAcc) { accPlotFile = DebugPlot.Instance.StartPlotting("Assets/Plotting/plots/acc.tsv"); await accPlotFile.WriteDataLine("timestamp", new[] {"x", "y", "z"}); } if (SensorConfig.plotEcg) { ecgPlotFile = DebugPlot.Instance.StartPlotting("Assets/Plotting/plots/ecg.tsv"); await ecgPlotFile.WriteDataLine("timestamp", new[] {"voltage"}); } if (SensorConfig.plotAcc || SensorConfig.plotEcg) { DebugPlot.Instance.ShowPlots(); } connection = new UdpConnection(SensorConfig.ipAddress, SensorConfig.port, OnAccData, OnEcgData); connection.Listen(); } public void Dispose() { connection.StopListening(); DebugPlot.DestroyInstance(); } private async void OnAccData(AccData data) { if (timeSync.DifStartAccStart < 0) { timeSync.DifStartAccStart = (long) (DateTime.Now - timeSync.StartTime).TotalMilliseconds; } Debug.Log($"#ACC DATA items: {data.Values.Count}"); await Task.WhenAll(UpdateSensorDataForAcc(data), PlotAcc(data)); } private async Task PlotAcc(AccData data) { if (startTimeAcc < 0) { startTimeAcc = data.Timestamp; } if (SensorConfig.plotAcc) { var internalTimestamp = (data.Timestamp - startTimeAcc) / 1000000; var timestamp = internalTimestamp + timeSync.DifStartAccStart; foreach (var item in data.Values) { await accPlotFile.WriteDataLine(timestamp, new[] {item.x, item.y, item.z}); timestamp += 1000 / SensorConfig.accSampleRate; } } } private async Task UpdateSensorDataForAcc(AccData data) { foreach (var item in data.Values) { sensorData.Acc = item; await Task.Delay(1000 / SensorConfig.accSampleRate); } } private async void OnEcgData(EcgData data) { if (timeSync.DifStartEcgStart < 0) { timeSync.DifStartEcgStart = (long) (DateTime.Now - timeSync.StartTime).TotalMilliseconds; } if (startTimeEcg < 0) { startTimeEcg = data.Timestamp; } if (SensorConfig.plotEcg) { var internalTimestamp = (data.Timestamp - startTimeEcg) / 1000000; var timestamp = internalTimestamp + timeSync.DifStartEcgStart; foreach (var item in data.Values) { await ecgPlotFile.WriteDataLine(timestamp, new[] {item}); timestamp += 1000 / ECG_SAMPLE_RATE; } } sensorData.EcgValue = data.Values[0]; //TODO } } }