PowerMeterReceiver.cs 5.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142
  1. using System.Collections.Generic;
  2. using ANT_Managed_Library;
  3. using UnityEngine;
  4. namespace Sensors.ANT
  5. {
  6. /*
  7. Frequencies such as 4.06 Hz (ANT+ Heart
  8. Rate) and 4.005 Hz (ANT+ Bike Power) will
  9. periodically “drift” into each other, or into
  10. other channel periods that may be present
  11. in the vicinity. During this overlap, channel
  12. collisions may occur as the radio can only
  13. service channel at a time.
  14. */
  15. public struct PowermeterSensorData
  16. {
  17. public int InstantaneousPower; // the instantaneous power in watt
  18. public int InstantaneousCadence; // crank cadence in RPM if available ( 255 indicates invalid)
  19. public float CrankTorque;
  20. }
  21. public class PowerMeterReceiver : AwaitDevice
  22. {
  23. private readonly Dictionary<int, int> sameEventCounter = new Dictionary<int, int> {{0x10, 0}, {0x12, 0}};
  24. private readonly Dictionary<int, int> updateEventCount = new Dictionary<int, int> {{0x10, 0}, {0x12, 0}};
  25. private AntChannel backgroundScanChannel;
  26. private AntChannel deviceChannel;
  27. private byte[] pageToSend;
  28. private int previousAccumulatedTorqueValue;
  29. private int previousTorqueEventCount;
  30. private PowermeterSensorData sensorData;
  31. private bool connected;
  32. public override int DeviceId { get; }
  33. public override bool Connected => connected;
  34. public PowermeterSensorData SensorData => sensorData;
  35. public PowerMeterReceiver()
  36. {
  37. }
  38. public PowerMeterReceiver(int deviceID)
  39. {
  40. DeviceId = deviceID;
  41. }
  42. public override void Connect(AntDevice device)
  43. {
  44. var channelID = AntManager.Instance.GetFreeChannelID();
  45. deviceChannel = AntManager.Instance.OpenChannel(ANT_ReferenceLibrary.ChannelType.ADV_Shared_0x20,
  46. channelID, (ushort) device.deviceNumber, device.deviceType, device.transType, (byte) device.radiofreq,
  47. (ushort) device.period, false);
  48. connected = true;
  49. deviceChannel.onReceiveData += Data;
  50. deviceChannel.onChannelResponse += ChannelResponse;
  51. deviceChannel.hideRXFAIL = true;
  52. }
  53. //Deal with the received Data
  54. private void Data(byte[] data)
  55. {
  56. /*if (data[0] == 0x10) // Standard Power-Only Main Data Page
  57. {
  58. Debug.Log($"Power only Main Data page [{string.Join(",", data)}]");
  59. if (data[1] == updateEventCount[0x10])
  60. sameEventCounter[0x10]++;
  61. else
  62. sameEventCounter[0x10] = 0;
  63. updateEventCount[0x10] = data[1];
  64. if (sameEventCounter[0x10] > 3)
  65. {
  66. sensorData.InstantaneousPower = 0;
  67. sensorData.InstantaneousCadence = 0;
  68. }
  69. else
  70. {
  71. sensorData.InstantaneousPower = data[6] | (data[7] << 8);
  72. sensorData.InstantaneousCadence = data[3];
  73. }
  74. }
  75. else*/ if (data[0] == 0x12) // Standard Crank Torque Main Data Page
  76. {
  77. Debug.Log($"Crank Torque Main Data page [{string.Join(",", data)}]");
  78. //Debug.Log($"Torque Data {data}");
  79. if (data[1] == updateEventCount[0x12])
  80. sameEventCounter[0x12]++;
  81. else
  82. sameEventCounter[0x12] = 0;
  83. updateEventCount[0x12] = data[1];
  84. Debug.Log($"UpdateEventCount = {data[1]}, same as before ? {updateEventCount[0x12] == data[1]}");
  85. Debug.Log($"Crank ticks = {data[2]}");
  86. Debug.Log($"Cadence = {data[3]}");
  87. if (sameEventCounter[0x12] > 3)
  88. {
  89. Debug.Log("SameEventCounter > 3 setting torque to 0");
  90. sensorData.CrankTorque = 0;
  91. }
  92. else
  93. {
  94. var accumulatedTorque = data[6] | (data[7] << 8);
  95. float divisor = 32 * (updateEventCount[0x12] - previousTorqueEventCount);
  96. if (divisor > 0)
  97. sensorData.CrankTorque = (accumulatedTorque - previousAccumulatedTorqueValue) / divisor;
  98. previousAccumulatedTorqueValue = accumulatedTorque;
  99. previousTorqueEventCount = updateEventCount[0x12];
  100. }
  101. }
  102. else
  103. {
  104. Debug.Log($"Other datapage got data: [{string.Join(",",data)}]");
  105. }
  106. }
  107. /*public void Calibrate()
  108. {
  109. Debug.Log("Sending : Manual Zero Calibration request");
  110. pageToSend = new byte[8] {0x01, 0xAA, 0x0FF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
  111. deviceChannel.sendAcknowledgedData(pageToSend);
  112. }*/
  113. private void ChannelResponse(ANT_Response response)
  114. {
  115. //Debug.Log($"response: {response.getChannelEventCode()}");
  116. //if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.EVENT_TRANSFER_TX_FAILED_0x06)
  117. // deviceChannel.sendAcknowledgedData(pageToSend); //send the page again if the transfer failed
  118. }
  119. }
  120. }