SpeedSensorReceiver.cs 4.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159
  1. using UnityEngine;
  2. using System.Collections;
  3. using System.Collections.Generic;
  4. using ANT_Managed_Library;
  5. using System;
  6. using UnityEngine.Serialization;
  7. [System.Serializable]
  8. public struct SpeedSensorConfig
  9. {
  10. public int deviceId;
  11. public float wheelCircumference;
  12. public SpeedSensorConfig(int deviceId = 0, float wheelCircumference = 2.096f)
  13. {
  14. this.deviceId = deviceId;
  15. this.wheelCircumference = wheelCircumference;
  16. }
  17. }
  18. public struct SpeedSensorData
  19. {
  20. public float Speed;
  21. public float SpeedKmh => Speed * 3.6f;
  22. public float Distance;
  23. }
  24. public class SpeedSensorReceiver
  25. {
  26. private AntChannel backgroundScanChannel;
  27. private AntChannel deviceChannel;
  28. private int stopRevCounterSpeed = 0;
  29. private int prevMeasTimeSpeed = 0;
  30. private int prevRevCountSpeed = 0;
  31. private int revCountZero = 0;
  32. private SpeedSensorData sensorData;
  33. public SpeedSensorConfig Config { get; }
  34. public bool Connected { get; private set; }
  35. public List<AntDevice> ScanResult { get; private set; }
  36. public SpeedSensorData SensorData => sensorData;
  37. public SpeedSensorReceiver()
  38. {
  39. }
  40. public SpeedSensorReceiver(SpeedSensorConfig config) => this.Config = config;
  41. public SpeedSensorReceiver(int deviceId) => Config = new SpeedSensorConfig(deviceId);
  42. public SpeedSensorReceiver(int deviceId, float wheelCircumference) =>
  43. Config = new SpeedSensorConfig(deviceId, wheelCircumference);
  44. //Start a background Scan to find the device
  45. public void StartScan()
  46. {
  47. Debug.Log("Looking for ANT + Speed sensor");
  48. AntManager.Instance.Init();
  49. ScanResult = new List<AntDevice>();
  50. backgroundScanChannel = AntManager.Instance.OpenBackgroundScanChannel(0);
  51. backgroundScanChannel.onReceiveData += ReceivedBackgroundScanData;
  52. }
  53. //If the device is found
  54. void ReceivedBackgroundScanData(Byte[] data)
  55. {
  56. byte deviceType = (data[12]); // extended info Device Type byte
  57. switch (deviceType)
  58. {
  59. case AntplusDeviceType.BikeSpeed:
  60. {
  61. int deviceNumber = (data[10]) | data[11] << 8;
  62. byte transType = data[13];
  63. foreach (AntDevice d in ScanResult)
  64. {
  65. if (d.deviceNumber == deviceNumber && d.transType == transType) //device already found
  66. return;
  67. }
  68. AntDevice foundDevice = new AntDevice();
  69. foundDevice.deviceType = deviceType;
  70. foundDevice.deviceNumber = deviceNumber;
  71. foundDevice.transType = transType;
  72. foundDevice.period = 8118;
  73. foundDevice.radiofreq = 57;
  74. foundDevice.name = "BikeSpeed(" + foundDevice.deviceNumber + ")";
  75. ScanResult.Add(foundDevice);
  76. if (deviceNumber == Config.deviceId)
  77. {
  78. Debug.Log($"Desired Speed Sensor with id {deviceNumber} found!");
  79. ConnectToDevice(foundDevice);
  80. }
  81. else
  82. {
  83. Debug.Log($"Speed sensor ({deviceNumber}) found and added to ScanResult");
  84. }
  85. break;
  86. }
  87. }
  88. }
  89. void ConnectToDevice(AntDevice device)
  90. {
  91. AntManager.Instance.CloseBackgroundScanChannel();
  92. byte channelID = AntManager.Instance.GetFreeChannelID();
  93. deviceChannel = AntManager.Instance.OpenChannel(ANT_ReferenceLibrary.ChannelType.BASE_Slave_Receive_0x00,
  94. channelID, (ushort) device.deviceNumber, device.deviceType, device.transType, (byte) device.radiofreq,
  95. (ushort) device.period, false);
  96. Connected = true;
  97. deviceChannel.onReceiveData += Data;
  98. deviceChannel.onChannelResponse += ChannelResponse;
  99. deviceChannel.hideRXFAIL = true;
  100. }
  101. //Deal with the received Data
  102. private void Data(Byte[] data)
  103. {
  104. //SPEED
  105. int measTime_speed = (data[4]) | data[5] << 8;
  106. int revCount_speed = (data[6]) | data[7] << 8;
  107. if (prevMeasTimeSpeed != 0 && measTime_speed != prevMeasTimeSpeed && prevMeasTimeSpeed < measTime_speed &&
  108. prevRevCountSpeed < revCount_speed)
  109. {
  110. sensorData.Speed = (Config.wheelCircumference * (revCount_speed - prevRevCountSpeed) * 1024) /
  111. (measTime_speed - prevMeasTimeSpeed);
  112. stopRevCounterSpeed = 0;
  113. }
  114. else
  115. stopRevCounterSpeed++;
  116. if (stopRevCounterSpeed >= 5)
  117. {
  118. stopRevCounterSpeed = 5;
  119. sensorData.Speed = 0;
  120. }
  121. prevMeasTimeSpeed = measTime_speed;
  122. prevRevCountSpeed = revCount_speed;
  123. //DISTANCE
  124. if (revCountZero == 0)
  125. revCountZero = revCount_speed;
  126. sensorData.Distance = Config.wheelCircumference * (revCount_speed - revCountZero);
  127. }
  128. void ChannelResponse(ANT_Response response)
  129. {
  130. }
  131. }