PolarReceiver.cs 5.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168
  1. using System;
  2. using System.Linq;
  3. using System.Threading.Tasks;
  4. using Plotting;
  5. using UnityEngine;
  6. using UnityEngine.Serialization;
  7. namespace Sensors.Polar
  8. {
  9. [System.Serializable]
  10. public struct PolarSensorConfig
  11. {
  12. public int port;
  13. public String ipAddress;
  14. public bool plotAcc;
  15. public bool plotEcg;
  16. public int accSampleRate; //TODO: let user choose between 25, 50, 100, 200
  17. public PolarSensorConfig(int port = 9099, string ipAddress = "0.0.0.0", bool plotAcc = false, bool plotEcg = false,
  18. int accSampleRate = 25)
  19. {
  20. this.port = port;
  21. this.ipAddress = ipAddress;
  22. this.plotAcc = plotAcc;
  23. this.plotEcg = plotEcg;
  24. this.accSampleRate = accSampleRate;
  25. }
  26. }
  27. public struct PolarSensorData
  28. {
  29. public Vector3 Acc;
  30. public float EcgValue;
  31. }
  32. public struct TimeSync
  33. {
  34. public DateTime StartTime;
  35. public long DifStartAccStart;
  36. public long DifStartEcgStart;
  37. }
  38. public class PolarReceiver
  39. {
  40. private UdpConnection connection;
  41. private PolarSensorData sensorData;
  42. public PolarSensorConfig SensorConfig { get; private set; }
  43. public PolarSensorData SensorData => sensorData;
  44. private PlotFileWriter ecgPlotFile;
  45. private PlotFileWriter accPlotFile;
  46. private static long startTimeAcc = -1;
  47. private static long startTimeEcg = -1;
  48. private static TimeSync timeSync;
  49. private const int ECG_SAMPLE_RATE = 130;
  50. [RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.BeforeSceneLoad)]
  51. private static void InitTime()
  52. {
  53. timeSync = new TimeSync {StartTime = DateTime.Now, DifStartAccStart = -1, DifStartEcgStart = -1};
  54. }
  55. public PolarReceiver(PolarSensorConfig config)
  56. {
  57. SensorConfig = config;
  58. }
  59. public async Task StartListening()
  60. {
  61. if (SensorConfig.plotAcc)
  62. {
  63. accPlotFile = DebugPlot.Instance.StartPlotting("Assets/Plotting/plots/acc.tsv");
  64. await accPlotFile.WriteDataLine("timestamp", new[] {"x", "y", "z"});
  65. }
  66. if (SensorConfig.plotEcg)
  67. {
  68. ecgPlotFile = DebugPlot.Instance.StartPlotting("Assets/Plotting/plots/ecg.tsv");
  69. await ecgPlotFile.WriteDataLine("timestamp", new[] {"voltage"});
  70. }
  71. if (SensorConfig.plotAcc || SensorConfig.plotEcg)
  72. {
  73. DebugPlot.Instance.ShowPlots();
  74. }
  75. connection = new UdpConnection(SensorConfig.ipAddress, SensorConfig.port, OnAccData, OnEcgData);
  76. connection.Listen();
  77. }
  78. public void Dispose()
  79. {
  80. connection.StopListening();
  81. DebugPlot.DestroyInstance();
  82. }
  83. private async void OnAccData(AccData data)
  84. {
  85. if (timeSync.DifStartAccStart < 0)
  86. {
  87. timeSync.DifStartAccStart = (long) (DateTime.Now - timeSync.StartTime).TotalMilliseconds;
  88. }
  89. Debug.Log($"#ACC DATA items: {data.Values.Count}");
  90. await Task.WhenAll(UpdateSensorDataForAcc(data), PlotAcc(data));
  91. }
  92. private async Task PlotAcc(AccData data)
  93. {
  94. if (startTimeAcc < 0)
  95. {
  96. startTimeAcc = data.Timestamp;
  97. }
  98. if (SensorConfig.plotAcc)
  99. {
  100. var internalTimestamp =
  101. (data.Timestamp - startTimeAcc) / 1000000;
  102. var timestamp = internalTimestamp + timeSync.DifStartAccStart;
  103. foreach (var item in data.Values)
  104. {
  105. await accPlotFile.WriteDataLine(timestamp, new[] {item.x, item.y, item.z});
  106. timestamp += 1000 / SensorConfig.accSampleRate;
  107. }
  108. }
  109. }
  110. private async Task UpdateSensorDataForAcc(AccData data)
  111. {
  112. foreach (var item in data.Values)
  113. {
  114. sensorData.Acc = item;
  115. await Task.Delay(1000 / SensorConfig.accSampleRate);
  116. }
  117. }
  118. private async void OnEcgData(EcgData data)
  119. {
  120. if (timeSync.DifStartEcgStart < 0)
  121. {
  122. timeSync.DifStartEcgStart = (long) (DateTime.Now - timeSync.StartTime).TotalMilliseconds;
  123. }
  124. if (startTimeEcg < 0)
  125. {
  126. startTimeEcg = data.Timestamp;
  127. }
  128. if (SensorConfig.plotEcg)
  129. {
  130. var internalTimestamp =
  131. (data.Timestamp - startTimeEcg) / 1000000;
  132. var timestamp = internalTimestamp + timeSync.DifStartEcgStart;
  133. foreach (var item in data.Values)
  134. {
  135. await ecgPlotFile.WriteDataLine(timestamp, new[] {item});
  136. timestamp += 1000 / ECG_SAMPLE_RATE;
  137. }
  138. }
  139. sensorData.EcgValue = data.Values[0]; //TODO
  140. }
  141. }
  142. }