SpeedSensorReceiver.cs 3.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121
  1. using System;
  2. using System.Collections.Generic;
  3. using ANT_Managed_Library;
  4. using UnityEngine;
  5. namespace Sensors.ANT
  6. {
  7. [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: AwaitDevice
  25. {
  26. private AntChannel backgroundScanChannel;
  27. private AntChannel deviceChannel;
  28. private int prevMeasTimeSpeed;
  29. private int prevRevCountSpeed;
  30. private int revCountZero;
  31. private SpeedSensorData sensorData;
  32. private int stopRevCounterSpeed;
  33. private bool connected;
  34. public SpeedSensorConfig Config { get; }
  35. public override bool Connected => connected;
  36. public SpeedSensorData SensorData => sensorData;
  37. public override int DeviceId => Config.deviceId;
  38. public SpeedSensorReceiver()
  39. {
  40. }
  41. public SpeedSensorReceiver(SpeedSensorConfig config)
  42. {
  43. Config = config;
  44. }
  45. public SpeedSensorReceiver(int deviceId)
  46. {
  47. Config = new SpeedSensorConfig(deviceId);
  48. }
  49. public SpeedSensorReceiver(int deviceId, float wheelCircumference)
  50. {
  51. Config = new SpeedSensorConfig(deviceId, wheelCircumference);
  52. }
  53. public override void Connect(AntDevice device)
  54. {
  55. var channelID = AntManager.Instance.GetFreeChannelID();
  56. deviceChannel = AntManager.Instance.OpenChannel(ANT_ReferenceLibrary.ChannelType.BASE_Slave_Receive_0x00,
  57. channelID, (ushort) device.deviceNumber, device.deviceType, device.transType, (byte) device.radiofreq,
  58. (ushort) device.period, false);
  59. connected = true;
  60. deviceChannel.onReceiveData += Data;
  61. deviceChannel.onChannelResponse += ChannelResponse;
  62. deviceChannel.hideRXFAIL = false;
  63. }
  64. //Deal with the received Data
  65. private void Data(byte[] data)
  66. {
  67. //SPEED
  68. var measTime_speed = data[4] | (data[5] << 8);
  69. var revCount_speed = data[6] | (data[7] << 8);
  70. if (prevMeasTimeSpeed != 0 && measTime_speed != prevMeasTimeSpeed && prevMeasTimeSpeed < measTime_speed &&
  71. prevRevCountSpeed < revCount_speed)
  72. {
  73. sensorData.Speed = Config.wheelCircumference * (revCount_speed - prevRevCountSpeed) * 1024 /
  74. (measTime_speed - prevMeasTimeSpeed);
  75. stopRevCounterSpeed = 0;
  76. }
  77. else
  78. {
  79. stopRevCounterSpeed++;
  80. }
  81. if (stopRevCounterSpeed >= 5)
  82. {
  83. stopRevCounterSpeed = 5;
  84. sensorData.Speed = 0;
  85. }
  86. prevMeasTimeSpeed = measTime_speed;
  87. prevRevCountSpeed = revCount_speed;
  88. //DISTANCE
  89. if (revCountZero == 0)
  90. revCountZero = revCount_speed;
  91. sensorData.Distance = Config.wheelCircumference * (revCount_speed - revCountZero);
  92. }
  93. private void ChannelResponse(ANT_Response response)
  94. {
  95. }
  96. }
  97. }